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 {
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]
}
app/src/lib/stores/featureFlags.ts
+8 -8
View File
@@ -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
}
esp32/include/kinematics.h
+26 -26
View File
@@ -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
}
};