From d2d1c85f5057815559b27775600a8d6eb231969c Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Tue, 21 May 2024 14:57:38 +0200 Subject: [PATCH] =?UTF-8?q?=F0=9F=A6=BE=20Makes=20embedded=20kinematics=20?= =?UTF-8?q?kinda=20work?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/src/lib/components/Visualization.svelte | 45 +++- app/src/lib/kinematic.ts | 14 +- app/src/lib/stores/model-store.ts | 2 +- esp32/lib/ESP32-sveltekit/Kinematics.h | 220 +++++++++++--------- esp32/lib/ESP32-sveltekit/MotionService.h | 41 ++-- 5 files changed, 195 insertions(+), 127 deletions(-) diff --git a/app/src/lib/components/Visualization.svelte b/app/src/lib/components/Visualization.svelte index bab7b18..534f44a 100644 --- a/app/src/lib/components/Visualization.svelte +++ b/app/src/lib/components/Visualization.svelte @@ -2,13 +2,15 @@ import { onDestroy, onMount } from 'svelte'; import { BufferGeometry, Line, LineBasicMaterial, Vector3, type NormalBufferAttributes } from 'three'; import uzip from 'uzip'; - import { model, servoAnglesOut } from '$lib/stores'; + import { 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'; import SceneBuilder from '$lib/sceneBuilder'; import { lerp, degToRad } from 'three/src/math/MathUtils'; import { GUI } from 'three/addons/libs/lil-gui.module.min.js'; + import Kinematic, { type position_t } from '$lib/kinematic'; + import { radToDeg } from 'three/src/math/MathUtils.js'; export let sky = true export let orbit = false @@ -27,7 +29,10 @@ let feet_trace = new Array(4).fill([]); let trace_lines: BufferGeometry[] = [] + let kinematic = new Kinematic() + let settings = { + 'Internal kinematic':false, 'Trace feet':debug, 'Trace points': 30, 'Fix camera on robot': true @@ -38,10 +43,45 @@ await createScene(); if (!isEmbeddedApp && panel) createPanel(); servoAngles.subscribe(updateAnglesFromStore) + outControllerData.subscribe((buffer) => { + if (!settings['Internal kinematic']) return + + const data = { + stop: buffer[0], + lx: buffer[1], + ly: buffer[2], + rx: buffer[3], + ry: buffer[4], + h: buffer[5], + s: buffer[6], + }; + + const Lp = [ + [100, -100, 100, 1], + [100, -100, -100, 1], + [-100, -100, 100, 1], + [-100, -100, -100, 1], + ]; + + const dir = [-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1] + + const position:position_t = { + omega: 0, + phi: data.rx / 4, + psi: data.ry / 4, + xm: data.ly / 2, + ym: (data.h+128)*0.75, + zm: data.lx / 2 + } + + let new_angles = kinematic.calcIK(Lp, position).map((x, i) => radToDeg(x * dir[i])); + modelTargetAngles = new_angles; + }) }); const updateAnglesFromStore = (angles: number[]) => { if (sceneManager.isDragging) return + if (settings['Internal kinematic']) return modelTargetAngles = angles; } @@ -54,6 +94,9 @@ gui_panel = new GUI({width: 310}); gui_panel.close(); gui_panel.domElement.id = 'three-gui-panel'; + + const general = gui_panel.addFolder('General'); + general.add(settings, 'Internal kinematic') const visibility = gui_panel.addFolder('Visualization'); visibility.add(settings, 'Trace feet') diff --git a/app/src/lib/kinematic.ts b/app/src/lib/kinematic.ts index 5887cfa..c698e4b 100644 --- a/app/src/lib/kinematic.ts +++ b/app/src/lib/kinematic.ts @@ -78,14 +78,18 @@ export default class Kinematic { ]; } - public calcIK(Lp: number[][], position: position_t): number[][] { + public calcIK(Lp: number[][], position: position_t): number[] { this.bodyIK(position); return [ - this.legIK(this.multiplyVector(this.inverse(this.Tlf), Lp[0])), - this.legIK(this.multiplyVector(this.Ix, this.multiplyVector(this.inverse(this.Trf), Lp[1]))), - this.legIK(this.multiplyVector(this.inverse(this.Tlb), Lp[2])), - this.legIK(this.multiplyVector(this.Ix, this.multiplyVector(this.inverse(this.Trb), Lp[3]))) + ...this.legIK(this.multiplyVector(this.inverse(this.Tlf), Lp[0])), + ...this.legIK( + this.multiplyVector(this.Ix, this.multiplyVector(this.inverse(this.Trf), Lp[1])) + ), + ...this.legIK(this.multiplyVector(this.inverse(this.Tlb), Lp[2])), + ...this.legIK( + this.multiplyVector(this.Ix, this.multiplyVector(this.inverse(this.Trb), Lp[3])) + ) ]; } diff --git a/app/src/lib/stores/model-store.ts b/app/src/lib/stores/model-store.ts index ee02291..362e593 100644 --- a/app/src/lib/stores/model-store.ts +++ b/app/src/lib/stores/model-store.ts @@ -21,7 +21,7 @@ export enum ModesEnum { export const mode: Writable = writable(ModesEnum.Idle); -export const outControllerData = writable(new Array([0, 0, 0, 0, 0, 70, 0])); +export const outControllerData = writable([0, 0, 0, 0, 0, 70, 0]); export const input: Writable = writable({ left: { x: 0, y: 0 }, diff --git a/esp32/lib/ESP32-sveltekit/Kinematics.h b/esp32/lib/ESP32-sveltekit/Kinematics.h index c205aa3..e71d01d 100644 --- a/esp32/lib/ESP32-sveltekit/Kinematics.h +++ b/esp32/lib/ESP32-sveltekit/Kinematics.h @@ -4,14 +4,55 @@ #include #include +static esp_err_t inverse(float a[4][4], float b[4][4]) + { + float s0 = a[0][0] * a[1][1] - a[1][0] * a[0][1]; + float s1 = a[0][0] * a[1][2] - a[1][0] * a[0][2]; + float s2 = a[0][0] * a[1][3] - a[1][0] * a[0][3]; + float s3 = a[0][1] * a[1][2] - a[1][1] * a[0][2]; + float s4 = a[0][1] * a[1][3] - a[1][1] * a[0][3]; + float s5 = a[0][2] * a[1][3] - a[1][2] * a[0][3]; + + float c5 = a[2][2] * a[3][3] - a[3][2] * a[2][3]; + float c4 = a[2][1] * a[3][3] - a[3][1] * a[2][3]; + float c3 = a[2][1] * a[3][2] - a[3][1] * a[2][2]; + float c2 = a[2][0] * a[3][3] - a[3][0] * a[2][3]; + float c1 = a[2][0] * a[3][2] - a[3][0] * a[2][2]; + float c0 = a[2][0] * a[3][1] - a[3][0] * a[2][1]; + + // Should check for 0 determinant + float det = (s0 * c5 - s1 * c4 + s2 * c3 + s3 * c2 - s4 * c1 + s5 * c0); + + if (det == 0.0) return ESP_FAIL; + + float invdet = 1.0 / det; + + + b[0][0] = ( a[1][1] * c5 - a[1][2] * c4 + a[1][3] * c3) * invdet; + b[0][1] = (-a[0][1] * c5 + a[0][2] * c4 - a[0][3] * c3) * invdet; + b[0][2] = ( a[3][1] * s5 - a[3][2] * s4 + a[3][3] * s3) * invdet; + b[0][3] = (-a[2][1] * s5 + a[2][2] * s4 - a[2][3] * s3) * invdet; + + b[1][0] = (-a[1][0] * c5 + a[1][2] * c2 - a[1][3] * c1) * invdet; + b[1][1] = ( a[0][0] * c5 - a[0][2] * c2 + a[0][3] * c1) * invdet; + b[1][2] = (-a[3][0] * s5 + a[3][2] * s2 - a[3][3] * s1) * invdet; + b[1][3] = ( a[2][0] * s5 - a[2][2] * s2 + a[2][3] * s1) * invdet; + + b[2][0] = ( a[1][0] * c4 - a[1][1] * c2 + a[1][3] * c0) * invdet; + b[2][1] = (-a[0][0] * c4 + a[0][1] * c2 - a[0][3] * c0) * invdet; + b[2][2] = ( a[3][0] * s4 - a[3][1] * s2 + a[3][3] * s0) * invdet; + b[2][3] = (-a[2][0] * s4 + a[2][1] * s2 - a[2][3] * s0) * invdet; + + b[3][0] = (-a[1][0] * c3 + a[1][1] * c1 - a[1][2] * c0) * invdet; + b[3][1] = ( a[0][0] * c3 - a[0][1] * c1 + a[0][2] * c0) * invdet; + b[3][2] = (-a[3][0] * s3 + a[3][1] * s1 - a[3][2] * s0) * invdet; + b[3][3] = ( a[2][0] * s3 - a[2][1] * s1 + a[2][2] * s0) * invdet; + + return ESP_OK; + } + typedef struct { - float omega; - float phi; - float psi; - float xm; - float ym; - float zm; - bool set; + float omega, phi, psi, xm, ym, zm; } position_t; @@ -21,18 +62,26 @@ typedef struct { class Kinematics { private: - dspm::Mat Tlf; - dspm::Mat Trf; - dspm::Mat Tlb; - dspm::Mat Trb; + float Trb[4][4] = {0,}; + float Trf[4][4] = {0,}; + float Tlb[4][4] = {0,}; + float Tlf[4][4] = {0,}; - float inverse[4][4]; + const float Ix[4][4] = {{-1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}}; + float inv[4][4]; + float point[4]; + float Q1[4][4]; - dspm::Mat Ix; + + const float sHp = sin(PI / 2); + const float cHp = cos(PI / 2); + + float point_lf[4][4]; public: float l1, l2, l3, l4; float L, W; + Kinematics(){ l1 = 50; l2 = 20; @@ -45,31 +94,29 @@ public: ~Kinematics(){} esp_err_t calculate_inverse_kinematics(float lp[4][4], position_t p, float result[12]) { + esp_err_t ret = ESP_OK; - Tlf.clear(); - Trf.clear(); - Tlb.clear(); - Trb.clear(); + ret = bodyIK(p); - float Ix_data[] = {-1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}; - dspm::Mat Ix(Ix_data, 4, 4); - - esp_err_t res = bodyIK(p); + ret += inverse(Tlf, inv); + dspm_mult_f32_ae32((float*) inv, (float*) lp[0], (float*) point, 4, 4, 1); + legIK((float*) point, result); - dspm::Mat result_vec(4, 4); - result_vec = (Tlf.inverse() * dspm::Mat(lp[0], 4, 1)); - legIK(result_vec.data, result); + ret += inverse(Trf, inv); + dspm_mult_f32_ae32((float*) Ix, (float*) inv, (float*) Q1, 4, 4, 4); + dspm_mult_f32_ae32((float*) Q1, (float*) lp[1], (float*) point, 4, 4, 1); + legIK((float*) point, result + 3); - result_vec = Ix * (Trf.inverse() * dspm::Mat(lp[1], 4, 1)); - legIK(result_vec.data, result + 3); + ret += inverse(Tlb, inv); + dspm_mult_f32_ae32((float*) inv, (float*) lp[2], (float*) point, 4, 4, 1); + legIK((float*) point, result + 6); - result_vec = (Tlb.inverse() * dspm::Mat(lp[2], 4, 1)); - legIK(result_vec.data, result + 6); + ret += inverse(Trb, inv); + dspm_mult_f32_ae32((float*) Ix, (float*) inv, (float*) Q1, 4, 4, 4); + dspm_mult_f32_ae32((float*) Q1, (float*) lp[3], (float*) point, 4, 4, 1); + legIK((float*) point, result + 9); - result_vec = Ix * (Trb.inverse() * dspm::Mat(lp[3], 4, 1)); - legIK(result_vec.data, result + 9); - - return res; + return ret; } esp_err_t bodyIK(position_t p) { @@ -79,97 +126,64 @@ public: float sin_phi = sin(p.phi*DEGREES2RAD); float cos_psi = cos(p.psi*DEGREES2RAD); float sin_psi = sin(p.psi*DEGREES2RAD); - - float Rx_data[] = { - 1, 0, 0, 0, - 0, cos_omega, -sin_omega, 0, - 0, sin_omega, cos_omega, 0, - 0, 0, 0, 1 + + float Tm[4][4] = { + {cos_phi * cos_psi, -sin_psi * cos_phi, sin_phi, p.xm}, + {sin_omega * sin_phi * cos_psi + sin_psi * cos_omega, -sin_omega * sin_phi * sin_psi + cos_omega * cos_psi, -sin_omega * cos_phi, p.ym}, + {sin_omega * sin_psi - sin_phi * cos_omega * cos_psi, sin_omega * cos_psi + sin_phi * sin_psi * cos_omega, cos_omega * cos_phi, p.zm}, + {0, 0, 0, 1} }; - dspm::Mat Rx(Rx_data, 4, 4); - float Ry_data[] = { - cos_phi, 0, sin_phi, 0, - 0, 1, 0, 0, - -sin_phi, 0, cos_phi, 0, - 0, 0, 0, 1 + float point_lf[4][4] = { + {cHp, 0, sHp, L / 2}, + {0, 1, 0, 0}, + {-sHp, 0, cHp, W / 2}, + {0, 0, 0, 1} }; - dspm::Mat Ry(Ry_data, 4, 4); - float Rz_data[] = { - cos_psi, -sin_psi, 0, 0, - sin_psi, cos_psi, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1 + float point_rf[4][4] = { + {cHp, 0, sHp, L / 2}, + {0, 1, 0, 0}, + {-sHp, 0, cHp, -W / 2}, + {0, 0, 0, 1} }; - dspm::Mat Rz(Rz_data, 4, 4); - dspm::Mat Rxyz = Rx * Ry * Rz; - - float T_data[] = { - 0, 0, 0, p.xm, - 0, 0, 0, p.ym, - 0, 0, 0, p.zm, - 0, 0, 0, 0 + float point_lb[4][4] = { + {cHp, 0, sHp, -L / 2}, + {0, 1, 0, 0}, + {-sHp, 0, cHp, W / 2}, + {0, 0, 0, 1} }; - dspm::Mat T(T_data, 4, 4); - dspm::Mat Tm = T + Rxyz; - - float sHp = sin(M_PI / 2); - float cHp = cos(M_PI / 2); - - float points_lf[] = { - cHp, 0, sHp, L / 2, - 0, 1, 0, 0, - -sHp, 0, cHp, W / 2, - 0, 0, 0, 1 + float point_rb[4][4] = { + {cHp, 0, sHp, -L / 2}, + {0, 1, 0, 0}, + {-sHp, 0, cHp, -W / 2}, + {0, 0, 0, 1} }; - Tlf = Tm * dspm::Mat(points_lf, 4, 4); - float points_rf[] = { - cHp, 0, sHp, L / 2, - 0, 1, 0, 0, - -sHp, 0, cHp, -W / 2, - 0, 0, 0, 1 - }; - Trf = Tm * dspm::Mat(points_rf, 4, 4); - - float points_lb[] = { - cHp, 0, sHp, -L / 2, - 0, 1, 0, 0, - -sHp, 0, cHp, W / 2, - 0, 0, 0, 1 - }; - Tlb = Tm * dspm::Mat(points_lb, 4, 4); - - float points_rb[] = { - cHp, 0, sHp, -L / 2, - 0, 1, 0, 0, - -sHp, 0, cHp, -W / 2, - 0, 0, 0, 1 - }; - Trb = Tm * dspm::Mat(points_rb, 4, 4); + dspm_mult_f32_ae32((float*) Tm, (float*) point_lf, (float*) Trb, 4, 4, 4); + dspm_mult_f32_ae32((float*) Tm, (float*) point_rf, (float*) Trf, 4, 4, 4); + dspm_mult_f32_ae32((float*) Tm, (float*) point_lb, (float*) Tlf, 4, 4, 4); + dspm_mult_f32_ae32((float*) Tm, (float*) point_rb, (float*) Tlb, 4, 4, 4); return ESP_OK; } void legIK(float point[4], float result[3]) { - float x = point[0]; - float y = point[1]; - float z = point[2]; + float x = point[0], y = point[1], z = point[2]; float F = sqrt(x * x + y * y - l1 * l1); if (isnan(F)) F = l1; float G = F - l2; float H = sqrt(G * G + z * z); - result[0] = -atan2(y, x) - atan2(F, -l1); - result[2] = acos((H * H - l3 * l3 - l4 * l4) / (2 * l3 * l4)); - if (isnan(result[2])) result[2] = 0; - result[1] = atan2(z, G) - atan2(l4 * sin(result[2]), l3 + l4 * cos(result[2])); - result[0] *= RAD2DEGREES; - result[1] *= RAD2DEGREES; - result[2] *= RAD2DEGREES; + float theta1 = -atan2(y, x) - atan2(F, -l1); + float theta3 = acos((H * H - l3 * l3 - l4 * l4) / (2 * l3 * l4)); + if (isnan(theta3)) theta3 = 0; + float theta2 = atan2(z, G) - atan2(l4 * sin(theta3), l3 + l4 * cos(theta3)); + result[0] = theta1 * RAD2DEGREES; + result[1] = theta2 * RAD2DEGREES; + result[2] = theta3 * RAD2DEGREES; } }; diff --git a/esp32/lib/ESP32-sveltekit/MotionService.h b/esp32/lib/ESP32-sveltekit/MotionService.h index 81a22d2..900dcaf 100644 --- a/esp32/lib/ESP32-sveltekit/MotionService.h +++ b/esp32/lib/ESP32-sveltekit/MotionService.h @@ -59,7 +59,7 @@ class MotionService { input[i] = array[i]; } - ESP_LOGI("MotionService", "Input: %d %d %d %d %d %d %d", input[0], input[1], input[2], input[3], input[4], input[5], input[6]); + 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]); } void handleMode(JsonObject &root, int originId) @@ -73,13 +73,13 @@ class MotionService void syncAngles(const String &originId = "", bool sync = false) { char output[100]; - sprintf(output, "[%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d]", angles[0], angles[1], angles[2], angles[3], angles[4], + 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()); } - int lerp(int start, int end, float t) { + float lerp(float start, float end, float t) { return (1 - t) * start + t * end; } @@ -90,7 +90,7 @@ class MotionService break; case MOTION_STATE::REST: for (int i = 0; i < 12; i++) { - int16_t new_angle = lerp(angles[i], rest_angles[i], 0.5); + float new_angle = lerp(angles[i], rest_angles[i], 0.5); if (new_angle != angles[i]) { angles[i] = new_angle; updated = true; @@ -106,27 +106,34 @@ class MotionService {-100, -100, 100, 1}, {-100, -100, -100, 1} }; - float lx = static_cast(input[1]); - float ly = static_cast(input[2]); - float rx = static_cast(input[3]); - float ry = static_cast(input[4]); - float h = static_cast(input[5]); - float s = static_cast(input[6]); - position_t p = {0, rx / 4, ry / 4, ly / 2, h, lx / 2, input[0]}; + 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); for (int i = 0; i < 12; i++) { - int16_t new_angle = lerp(angles[i], new_angles[i] * dir[i], 0.3); + float new_angle = lerp(angles[i], new_angles[i] * dir[i], 0.3); if (new_angle != angles[i]) { angles[i] = new_angle; updated = true; } } if (updated) { - ESP_LOGI("MotionService", "New angles: %f %f %f %f %f %f %f %f %f %f %f %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]); + ESP_LOGI("MotionService", "New angles: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f", angles[0], angles[1], angles[2], angles[3], angles[4], angles[5], angles[6], angles[7], angles[8], angles[9], angles[10], angles[11]); } break; } @@ -156,10 +163,10 @@ class MotionService constexpr static int MotionInterval = 100; - int8_t input[7] = {0, 0, 0, 0, 0, 0, 0}; - int16_t angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; - int16_t rest_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145}; - int16_t stand_angles[12] = {0, 45, -90, 0, 45, -90, 0, 45, -90, 0, 45, -90}; + 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;