From b338ec031651b335aa9b99304c4a0dcff5491d03 Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Mon, 27 May 2024 01:23:10 +0200 Subject: [PATCH] =?UTF-8?q?=F0=9F=AA=84=20Update=20kinematic=20interface?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- esp32/lib/ESP32-sveltekit/Kinematics.h | 23 +++++-- esp32/lib/ESP32-sveltekit/MotionService.h | 80 ++++++++++++----------- 2 files changed, 59 insertions(+), 44 deletions(-) diff --git a/esp32/lib/ESP32-sveltekit/Kinematics.h b/esp32/lib/ESP32-sveltekit/Kinematics.h index b32fbd3..3b0b721 100644 --- a/esp32/lib/ESP32-sveltekit/Kinematics.h +++ b/esp32/lib/ESP32-sveltekit/Kinematics.h @@ -51,8 +51,17 @@ static esp_err_t inverse(float a[4][4], float b[4][4]) return ESP_OK; } -typedef struct { +typedef struct { float omega, phi, psi, xm, ym, zm; + float feet[4][4]; + + void updateFeet(const float newFeet[4][4]) { + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { + feet[i][j] = newFeet[i][j]; + } + } + } } body_state_t; @@ -93,27 +102,27 @@ public: } ~Kinematics(){} - esp_err_t calculate_inverse_kinematics(float lp[4][4], body_state_t p, float result[12]) { + esp_err_t calculate_inverse_kinematics(body_state_t body_state, float result[12]) { esp_err_t ret = ESP_OK; - ret = bodyIK(p); + ret = bodyIK(body_state); ret += inverse(Tlf, inv); - dspm_mult_f32_ae32((float*) inv, (float*) lp[0], (float*) point, 4, 4, 1); + dspm_mult_f32_ae32((float*) inv, (float*) body_state.feet[0], (float*) point, 4, 4, 1); legIK((float*) point, 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); + dspm_mult_f32_ae32((float*) Q1, (float*) body_state.feet[1], (float*) point, 4, 4, 1); legIK((float*) point, result + 3); ret += inverse(Tlb, inv); - dspm_mult_f32_ae32((float*) inv, (float*) lp[2], (float*) point, 4, 4, 1); + dspm_mult_f32_ae32((float*) inv, (float*) body_state.feet[2], (float*) point, 4, 4, 1); legIK((float*) point, 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); + dspm_mult_f32_ae32((float*) Q1, (float*) body_state.feet[3], (float*) point, 4, 4, 1); legIK((float*) point, result + 9); return ret; diff --git a/esp32/lib/ESP32-sveltekit/MotionService.h b/esp32/lib/ESP32-sveltekit/MotionService.h index f1e3ad3..4c45509 100644 --- a/esp32/lib/ESP32-sveltekit/MotionService.h +++ b/esp32/lib/ESP32-sveltekit/MotionService.h @@ -45,6 +45,8 @@ class MotionService _socket->onSubscribe(ANGLES_EVENT, std::bind(&MotionService::syncAngles, this, std::placeholders::_1, std::placeholders::_2)); + + body_state.updateFeet(default_feet_positions); } void anglesEvent(JsonObject &root, int originId) @@ -60,37 +62,41 @@ class MotionService 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]; + body_state.omega = array[0]; + body_state.phi = array[1]; + body_state.psi = array[2]; + body_state.xm = array[3]; + body_state.ym = array[4]; + body_state.zm = array[5]; // syncAngles(String(originId)); } void handleInput(JsonObject &root, int originId) { JsonArray array = root["data"].as(); - for (int i = 0; i < 7; i++) - { - input[i] = array[i]; + float lx = array[1]; + float ly = array[2]; + float rx = array[3]; + float ry = array[4]; + float h = array[5]; + float s = array[6]; + + body_state.ym = (h + 128) * (float)0.7; + + switch (motionState) { + case MOTION_STATE::STAND: { + body_state.phi = rx / 4; + body_state.psi = ry / 4; + body_state.xm = ly / 2; + body_state.zm = lx / 2; + break; + } + case MOTION_STATE::WALK: + position_target.x = rx / 2; + position_target.z = ry / 2; + position_target.yaw = lx / 2; + break; } - 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]); } void handleMode(JsonObject &root, int originId) @@ -117,21 +123,21 @@ class MotionService float new_angles[12] = {0,}; switch (motionState) { case MOTION_STATE::IDLE: + return false; break; + case MOTION_STATE::REST: update_angles(rest_angles, new_angles, false); break; case MOTION_STATE::STAND: { - kinematics.calculate_inverse_kinematics(lp, position, new_angles); + kinematics.calculate_inverse_kinematics(body_state, new_angles); break; } case MOTION_STATE::WALK: - lp[0][1] += walk_dir; - if (lp[0][1] >= 50) walk_dir = -1; - if (lp[0][1] <= -100) walk_dir = 1; + gaitPlanner.update_trajectory(position_target, MotionInterval, body_state); - kinematics.calculate_inverse_kinematics(lp, position, new_angles); + kinematics.calculate_inverse_kinematics(body_state, new_angles); break; } return update_angles(new_angles, angles); @@ -162,25 +168,25 @@ class MotionService SecurityManager *_securityManager; TaskManager *_taskManager; Kinematics kinematics; + GaitPlanner gaitPlanner; + MOTION_STATE motionState = MOTION_STATE::IDLE; + unsigned long _lastUpdate; constexpr static int MotionInterval = 100; - body_state_t position = {0, 0, 0, 0, 0, 0}; + body_state_t body_state = {0,}; + position_target_t position_target = {0,}; + float dir[12] = {-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1}; - float lp[4][4] = { + float default_feet_positions[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 angles[12] = {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 walk_dir = 2; }; #endif