From 6f46c1f59867bbeabe841db7f93ecf473955ab89 Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Wed, 3 Sep 2025 15:50:34 +0200 Subject: [PATCH] =?UTF-8?q?=F0=9F=8E=A8=20Renames=20kinematics=20config?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/src/lib/kinematic.ts | 37 ++++++++++----------- app/src/lib/stores/featureFlags.ts | 16 ++++----- esp32/include/kinematics.h | 52 +++++++++++++++--------------- 3 files changed, 53 insertions(+), 52 deletions(-) diff --git a/app/src/lib/kinematic.ts b/app/src/lib/kinematic.ts index 7ec3414..ec26baa 100644 --- a/app/src/lib/kinematic.ts +++ b/app/src/lib/kinematic.ts @@ -21,10 +21,10 @@ export interface target_position { } export interface KinematicParams { - l1: number - l2: number - l3: number - l4: number + coxa: number + coxa_offset: number + femur: number + tibia: number L: number W: number } @@ -34,10 +34,10 @@ const { cos, sin, atan2, acos, sqrt, max, min } = Math const DEG2RAD = 0.017453292519943 export default class Kinematic { - l1: number - l2: number - l3: number - l4: number + coxa: number + coxa_offset: number + femur: number + tibia: number L: number W: number @@ -53,10 +53,10 @@ export default class Kinematic { ] constructor(params: KinematicParams) { - this.l1 = params.l1 - this.l2 = params.l2 - this.l3 = params.l3 - this.l4 = params.l4 + this.coxa = params.coxa + this.coxa_offset = params.coxa_offset + this.femur = params.femur + this.tibia = params.tibia this.L = params.L this.W = params.W @@ -70,7 +70,7 @@ export default class Kinematic { getDefaultFeetPos(): number[][] { return this.mountOffsets.map((offset, i) => { - return [offset[0], -1, offset[2] + (i % 2 === 1 ? -this.l1 : this.l1)] + return [offset[0], -1, offset[2] + (i % 2 === 1 ? -this.coxa : this.coxa)] }) } @@ -113,13 +113,14 @@ export default class Kinematic { } private legIK(x: number, y: number, z: number): [number, number, number] { - const F = sqrt(max(0, x * x + y * y - this.l1 * this.l1)) - const G = F - this.l2 + const F = sqrt(max(0, x * x + y * y - this.coxa * this.coxa)) + const G = F - this.coxa_offset const H = sqrt(G * G + z * z) - const t1 = -atan2(y, x) - atan2(F, -this.l1) - const D = (H * H - this.l3 * this.l3 - this.l4 * this.l4) / (2 * this.l3 * this.l4) + const t1 = -atan2(y, x) - atan2(F, -this.coxa) + const D = + (H * H - this.femur * this.femur - this.tibia * this.tibia) / (2 * this.femur * this.tibia) const t3 = acos(max(-1, min(1, D))) - const t2 = atan2(z, G) - atan2(this.l4 * sin(t3), this.l3 + this.l4 * cos(t3)) + const t2 = atan2(z, G) - atan2(this.tibia * sin(t3), this.femur + this.tibia * cos(t3)) return [t1, t2, t3] } diff --git a/app/src/lib/stores/featureFlags.ts b/app/src/lib/stores/featureFlags.ts index c0cfcc5..f2177ed 100644 --- a/app/src/lib/stores/featureFlags.ts +++ b/app/src/lib/stores/featureFlags.ts @@ -27,10 +27,10 @@ export const variants = { model: `${base}/spot_micro.urdf.xacro`, stl: `${base}/stl.zip`, kinematics: { - l1: 60.5 / 100, - l2: 10 / 100, - l3: 111.7 / 100, - l4: 118.5 / 100, + coxa: 60.5 / 100, + coxa_offset: 10 / 100, + femur: 111.7 / 100, + tibia: 118.5 / 100, L: 207.5 / 100, W: 78 / 100 } @@ -39,10 +39,10 @@ export const variants = { model: `${base}/yertle.URDF`, stl: `${base}/URDF.zip`, kinematics: { - l1: 35 / 100, - l2: 0 / 100, - l3: 130 / 100, - l4: 130 / 100, + coxa: 35 / 100, + coxa_offset: 0 / 100, + femur: 130 / 100, + tibia: 130 / 100, L: 240 / 100, W: 78 / 100 } diff --git a/esp32/include/kinematics.h b/esp32/include/kinematics.h index c280d15..dcb0de5 100644 --- a/esp32/include/kinematics.h +++ b/esp32/include/kinematics.h @@ -22,26 +22,26 @@ struct alignas(16) body_state_t { class Kinematics { private: #if defined(SPOTMICRO_ESP32) - static constexpr float l1 = 60.5f / 100.0f; - static constexpr float l2 = 10.0f / 100.0f; - static constexpr float l3 = 111.2f / 100.0f; - static constexpr float l4 = 118.5f / 100.0f; + static constexpr float coxa = 60.5f / 100.0f; + static constexpr float coxa_offset = 10.0f / 100.0f; + static constexpr float femur = 111.2f / 100.0f; + static constexpr float tibia = 118.5f / 100.0f; static constexpr float L = 207.5f / 100.0f; static constexpr float W = 78.0f / 100.0f; #elif defined(SPOTMICRO_ESP32_MINI) - static constexpr float l1 = 0.01f / 100.0f; - static constexpr float l2 = 0.01f / 100.0f; - static constexpr float l3 = 52.0f / 100.0f; - static constexpr float l4 = 65.0f / 100.0f; + static constexpr float coxa = 35.0f / 100.0f; + static constexpr float coxa_offset = 0.0f / 100.0f; + static constexpr float femur = 52.0f / 100.0f; + static constexpr float tibia = 65.0f / 100.0f; static constexpr float L = 120.0f / 100.0f; static constexpr float W = 78.5f / 100.0f; #elif defined(SPOTMICRO_YERTLE) - static constexpr float l1 = 35.0f / 100.0f; - static constexpr float l2 = 0.0f; - static constexpr float l3 = 130.0f / 100.0f; - static constexpr float l4 = 130.0f / 100.0f; + static constexpr float coxa = 35.0f / 100.0f; + static constexpr float coxa_offset = 0.0f; + static constexpr float femur = 130.0f / 100.0f; + static constexpr float tibia = 130.0f / 100.0f; static constexpr float L = 240.0f / 100.0f; static constexpr float W = 78.0f / 100.0f; @@ -62,10 +62,10 @@ class Kinematics { public: static constexpr float default_feet_positions[4][4] = { - {mountOffsets[0][0], -1, mountOffsets[0][2] + l1, 1}, - {mountOffsets[1][0], -1, mountOffsets[1][2] - l1, 1}, - {mountOffsets[2][0], -1, mountOffsets[2][2] + l1, 1}, - {mountOffsets[3][0], -1, mountOffsets[3][2] - l1, 1}, + {mountOffsets[0][0], -1, mountOffsets[0][2] + coxa, 1}, + {mountOffsets[1][0], -1, mountOffsets[1][2] - coxa, 1}, + {mountOffsets[2][0], -1, mountOffsets[2][2] + coxa, 1}, + {mountOffsets[3][0], -1, mountOffsets[3][2] - coxa, 1}, }; esp_err_t calculate_inverse_kinematics(const body_state_t body_state, float result[12]) { @@ -153,21 +153,21 @@ class Kinematics { inv_rot[2][2] = rot[2][2]; } - inline void legIK(float x, float y, float z, float result[3]) { - float F = sqrt(max(0.0f, x * x + y * y - l1 * l1)); - float G = F - l2; + inline void legIK(float x, float y, float z, float out[3]) { + float F = sqrt(max(0.0f, x * x + y * y - coxa * coxa)); + float G = F - coxa_offset; float H = sqrt(G * G + z * z); - float theta1 = -atan2f(y, x) - atan2f(F, -l1); - float D = (H * H - l3 * l3 - l4 * l4) / (2 * l3 * l4); + float theta1 = -atan2f(y, x) - atan2f(F, -coxa); + float D = (H * H - femur * femur - tibia * tibia) / (2 * femur * tibia); float theta3 = acosf(max(-1.0f, min(1.0f, D))); - float theta2 = atan2f(z, G) - atan2f(l4 * sinf(theta3), l3 + l4 * cosf(theta3)); - result[0] = RAD_TO_DEG_F(theta1); - result[1] = RAD_TO_DEG_F(theta2); + float theta2 = atan2f(z, G) - atan2f(tibia * sinf(theta3), femur + tibia * cosf(theta3)); + out[0] = RAD_TO_DEG_F(theta1); + out[1] = RAD_TO_DEG_F(theta2); #if defined(SPOTMICRO_ESP32) || defined(SPOTMICRO_ESP32_MINI) - result[2] = RAD_TO_DEG_F(theta3); + out[2] = RAD_TO_DEG_F(theta3); #elif defined(SPOTMICRO_YERTLE) - result[2] = RAD_TO_DEG_F(theta3 + theta2); + out[2] = RAD_TO_DEG_F(theta3 + theta2); #endif } };