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

🦾 Makes embedded kinematics kinda work

Browse Source
This commit is contained in:
Rune Harlyk
2024-05-21 14:57:38 +02:00
committed by Rune Harlyk
parent 68d319e022
commit d2d1c85f50
5 changed files with 195 additions and 127 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/src/lib/components/Visualization.svelte
+44 -1
View File
@@ -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<NormalBufferAttributes>[] = []
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')
app/src/lib/kinematic.ts
+9 -5
View File
@@ -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]))
)
];
}
app/src/lib/stores/model-store.ts
+1 -1
View File
@@ -21,7 +21,7 @@ export enum ModesEnum {
export const mode: Writable<ModesEnum> = 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<ControllerInput> = writable({
left: { x: 0, y: 0 },
esp32/lib/ESP32-sveltekit/Kinematics.h
+117 -103
View File
@@ -4,14 +4,55 @@
#include <cmath>
#include <esp_dsp.h>
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;
}
};
esp32/lib/ESP32-sveltekit/MotionService.h
+24 -17
View File
@@ -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<float>(input[1]);
float ly = static_cast<float>(input[2]);
float rx = static_cast<float>(input[3]);
float ry = static_cast<float>(input[4]);
float h = static_cast<float>(input[5]);
float s = static_cast<float>(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;