Darkages/SpotMicroESP32-Leika
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

🎨 Renames kinematics config

Browse Source
This commit is contained in:
Rune Harlyk
2025-09-03 15:50:34 +02:00
committed by Rune Harlyk
parent bc810ee2dd
commit 6f46c1f598
3 changed files with 53 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/src/lib/kinematic.ts
+19 -18
View File
@@ -21,10 +21,10 @@ export interface target_position {
} }
export interface KinematicParams { export interface KinematicParams {
l1: number coxa: number
l2: number coxa_offset: number
l3: number femur: number
l4: number tibia: number
L: number L: number
W: number W: number
} }
@@ -34,10 +34,10 @@ const { cos, sin, atan2, acos, sqrt, max, min } = Math
const DEG2RAD = 0.017453292519943 const DEG2RAD = 0.017453292519943
export default class Kinematic { export default class Kinematic {
l1: number coxa: number
l2: number coxa_offset: number
l3: number femur: number
l4: number tibia: number
L: number L: number
W: number W: number
@@ -53,10 +53,10 @@ export default class Kinematic {
] ]
constructor(params: KinematicParams) { constructor(params: KinematicParams) {
this.l1 = params.l1 this.coxa = params.coxa
this.l2 = params.l2 this.coxa_offset = params.coxa_offset
this.l3 = params.l3 this.femur = params.femur
this.l4 = params.l4 this.tibia = params.tibia
this.L = params.L this.L = params.L
this.W = params.W this.W = params.W
@@ -70,7 +70,7 @@ export default class Kinematic {
getDefaultFeetPos(): number[][] { getDefaultFeetPos(): number[][] {
return this.mountOffsets.map((offset, i) => { 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] { 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 F = sqrt(max(0, x * x + y * y - this.coxa * this.coxa))
const G = F - this.l2 const G = F - this.coxa_offset
const H = sqrt(G * G + z * z) const H = sqrt(G * G + z * z)
const t1 = -atan2(y, x) - atan2(F, -this.l1) const t1 = -atan2(y, x) - atan2(F, -this.coxa)
const D = (H * H - this.l3 * this.l3 - this.l4 * this.l4) / (2 * this.l3 * this.l4) 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 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] return [t1, t2, t3]
} }
app/src/lib/stores/featureFlags.ts
+8 -8
View File
@@ -27,10 +27,10 @@ export const variants = {
model: `${base}/spot_micro.urdf.xacro`, model: `${base}/spot_micro.urdf.xacro`,
stl: `${base}/stl.zip`, stl: `${base}/stl.zip`,
kinematics: { kinematics: {
l1: 60.5 / 100, coxa: 60.5 / 100,
l2: 10 / 100, coxa_offset: 10 / 100,
l3: 111.7 / 100, femur: 111.7 / 100,
l4: 118.5 / 100, tibia: 118.5 / 100,
L: 207.5 / 100, L: 207.5 / 100,
W: 78 / 100 W: 78 / 100
} }
@@ -39,10 +39,10 @@ export const variants = {
model: `${base}/yertle.URDF`, model: `${base}/yertle.URDF`,
stl: `${base}/URDF.zip`, stl: `${base}/URDF.zip`,
kinematics: { kinematics: {
l1: 35 / 100, coxa: 35 / 100,
l2: 0 / 100, coxa_offset: 0 / 100,
l3: 130 / 100, femur: 130 / 100,
l4: 130 / 100, tibia: 130 / 100,
L: 240 / 100, L: 240 / 100,
W: 78 / 100 W: 78 / 100
} }
esp32/include/kinematics.h
+26 -26
View File
@@ -22,26 +22,26 @@ struct alignas(16) body_state_t {
class Kinematics { class Kinematics {
private: private:
#if defined(SPOTMICRO_ESP32) #if defined(SPOTMICRO_ESP32)
static constexpr float l1 = 60.5f / 100.0f; static constexpr float coxa = 60.5f / 100.0f;
static constexpr float l2 = 10.0f / 100.0f; static constexpr float coxa_offset = 10.0f / 100.0f;
static constexpr float l3 = 111.2f / 100.0f; static constexpr float femur = 111.2f / 100.0f;
static constexpr float l4 = 118.5f / 100.0f; static constexpr float tibia = 118.5f / 100.0f;
static constexpr float L = 207.5f / 100.0f; static constexpr float L = 207.5f / 100.0f;
static constexpr float W = 78.0f / 100.0f; static constexpr float W = 78.0f / 100.0f;
#elif defined(SPOTMICRO_ESP32_MINI) #elif defined(SPOTMICRO_ESP32_MINI)
static constexpr float l1 = 0.01f / 100.0f; static constexpr float coxa = 35.0f / 100.0f;
static constexpr float l2 = 0.01f / 100.0f; static constexpr float coxa_offset = 0.0f / 100.0f;
static constexpr float l3 = 52.0f / 100.0f; static constexpr float femur = 52.0f / 100.0f;
static constexpr float l4 = 65.0f / 100.0f; static constexpr float tibia = 65.0f / 100.0f;
static constexpr float L = 120.0f / 100.0f; static constexpr float L = 120.0f / 100.0f;
static constexpr float W = 78.5f / 100.0f; static constexpr float W = 78.5f / 100.0f;
#elif defined(SPOTMICRO_YERTLE) #elif defined(SPOTMICRO_YERTLE)
static constexpr float l1 = 35.0f / 100.0f; static constexpr float coxa = 35.0f / 100.0f;
static constexpr float l2 = 0.0f; static constexpr float coxa_offset = 0.0f;
static constexpr float l3 = 130.0f / 100.0f; static constexpr float femur = 130.0f / 100.0f;
static constexpr float l4 = 130.0f / 100.0f; static constexpr float tibia = 130.0f / 100.0f;
static constexpr float L = 240.0f / 100.0f; static constexpr float L = 240.0f / 100.0f;
static constexpr float W = 78.0f / 100.0f; static constexpr float W = 78.0f / 100.0f;
@@ -62,10 +62,10 @@ class Kinematics {
public: public:
static constexpr float default_feet_positions[4][4] = { static constexpr float default_feet_positions[4][4] = {
{mountOffsets[0][0], -1, mountOffsets[0][2] + l1, 1}, {mountOffsets[0][0], -1, mountOffsets[0][2] + coxa, 1},
{mountOffsets[1][0], -1, mountOffsets[1][2] - l1, 1}, {mountOffsets[1][0], -1, mountOffsets[1][2] - coxa, 1},
{mountOffsets[2][0], -1, mountOffsets[2][2] + l1, 1}, {mountOffsets[2][0], -1, mountOffsets[2][2] + coxa, 1},
{mountOffsets[3][0], -1, mountOffsets[3][2] - l1, 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]) { 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]; inv_rot[2][2] = rot[2][2];
} }
inline void legIK(float x, float y, float z, float result[3]) { inline void legIK(float x, float y, float z, float out[3]) {
float F = sqrt(max(0.0f, x * x + y * y - l1 * l1)); float F = sqrt(max(0.0f, x * x + y * y - coxa * coxa));
float G = F - l2; float G = F - coxa_offset;
float H = sqrt(G * G + z * z); float H = sqrt(G * G + z * z);
float theta1 = -atan2f(y, x) - atan2f(F, -l1); float theta1 = -atan2f(y, x) - atan2f(F, -coxa);
float D = (H * H - l3 * l3 - l4 * l4) / (2 * l3 * l4); float D = (H * H - femur * femur - tibia * tibia) / (2 * femur * tibia);
float theta3 = acosf(max(-1.0f, min(1.0f, D))); float theta3 = acosf(max(-1.0f, min(1.0f, D)));
float theta2 = atan2f(z, G) - atan2f(l4 * sinf(theta3), l3 + l4 * cosf(theta3)); float theta2 = atan2f(z, G) - atan2f(tibia * sinf(theta3), femur + tibia * cosf(theta3));
result[0] = RAD_TO_DEG_F(theta1); out[0] = RAD_TO_DEG_F(theta1);
result[1] = RAD_TO_DEG_F(theta2); out[1] = RAD_TO_DEG_F(theta2);
#if defined(SPOTMICRO_ESP32) || defined(SPOTMICRO_ESP32_MINI) #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) #elif defined(SPOTMICRO_YERTLE)
result[2] = RAD_TO_DEG_F(theta3 + theta2); out[2] = RAD_TO_DEG_F(theta3 + theta2);
#endif #endif
} }
}; };