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

🪠 Removes feature code

Browse Source
This commit is contained in:
Rune Harlyk
2024-06-03 18:43:01 +02:00
committed by Rune Harlyk
parent bfca33e55d
commit 4bf630edd3
9 changed files with 16742 additions and 16260 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/src/lib/components/Visualization.svelte
+7 -13
View File
@@ -45,7 +45,7 @@
];
let settings = {
'Internal kinematic':true,
'Internal kinematic':false,
'Robot transform controls':false,
'Auto orient robot':true,
'Trace feet':debug,
@@ -87,14 +87,6 @@
settings.x = data.ly / 2
settings.z = data.lx / 2
break;
case ModesEnum.Walk:
// target_position.yaw += data.lx / 100
// const angle = radToDeg(Math.atan2(data.rx, data.ry)) - 90 + $mpu.heading + target_position.yaw;
// const distance = Math.sqrt(data.rx**2 + data.ry**2) / 25
// target_position.x = distance * Math.cos(degToRad(angle))
// target_position.z = distance * Math.sin(degToRad(angle))
break;
}
})
});
@@ -267,6 +259,11 @@
settings.z = -robot.position.x * 100
}
const updateTargetPosition = () => {
target.position.x = lerp(target.position.x, target_position.x, 0.5)
target.position.z = lerp(target.position.z, target_position.z, 0.5)
}
const render = () => {
const robot = sceneManager.model;
if (!robot) return;
@@ -293,10 +290,7 @@
}
orient_robot(robot, toes)
target.position.x = lerp(target.position.x, target_position.x, 0.5)
// target.position.y = lerp(target.position.y, robot.position.y, 0.5)
target.position.z = lerp(target.position.z, target_position.z, 0.5)
updateTargetPosition();
};
app/src/lib/kinematic.ts
+7 -7
View File
@@ -33,7 +33,7 @@ export default class Kinematic {
L: number;
W: number;
DEGREES2RAD = 0.017453292519943;
DEG2RAD = 0.017453292519943;
sHp = sin(Math.PI / 2);
cHp = cos(Math.PI / 2);
@@ -115,12 +115,12 @@ export default class Kinematic {
}
bodyIK(p: body_state_t) {
const cos_omega = cos(p.omega * this.DEGREES2RAD);
const sin_omega = sin(p.omega * this.DEGREES2RAD);
const cos_phi = cos(p.phi * this.DEGREES2RAD);
const sin_phi = sin(p.phi * this.DEGREES2RAD);
const cos_psi = cos(p.psi * this.DEGREES2RAD);
const sin_psi = sin(p.psi * this.DEGREES2RAD);
const cos_omega = cos(p.omega * this.DEG2RAD);
const sin_omega = sin(p.omega * this.DEG2RAD);
const cos_phi = cos(p.phi * this.DEG2RAD);
const sin_phi = sin(p.phi * this.DEG2RAD);
const cos_psi = cos(p.psi * this.DEG2RAD);
const sin_psi = sin(p.psi * this.DEG2RAD);
const Tm: number[][] = [
[cos_phi * cos_psi, -sin_psi * cos_phi, sin_phi, p.xm],
app/src/lib/stores/model-store.ts
+1 -1
View File
@@ -19,7 +19,7 @@ export enum ModesEnum {
Walk
}
export const mode: Writable<ModesEnum> = writable(ModesEnum.Walk);
export const mode: Writable<ModesEnum> = writable(ModesEnum.Idle);
export const outControllerData = writable([0, 0, 0, 0, 0, 70, 0]);
esp32/build/firmware/Spot-Micro_esp32cam_0-0-1.bin
BIN
View File
Binary file not shown.
esp32/build/firmware/Spot-Micro_esp32cam_0-0-1.md5
-1
View File
@@ -1 +0,0 @@
e26accd60ea0ebc13e6ab02a735aa5ee
esp32/lib/ESP32-sveltekit/IMUService.h
+2 -3
View File
@@ -1,11 +1,11 @@
#pragma once
#include "MPU6050_6Axis_MotionApps612.h"
#include <Adafruit_Sensor.h>
#include <Adafruit_BMP085_U.h>
#include <Adafruit_HMC5883_U.h>
#include <Adafruit_Sensor.h>
#include <ArduinoJson.h>
#include <EventSocket.h>
#include <MPU6050_6Axis_MotionApps612.h>
#define IMU_INTERVAL 500
#define MAX_ESP_IMU_SIZE 500
@@ -149,7 +149,6 @@ protected:
doc["x"] = round2(getAngleX());
doc["y"] = round2(getAngleY());
doc["z"] = round2(getAngleZ());
// doc["imu_temp"] = round2(getTemp());
}
#endif
#if FT_ENABLED(FT_MAG)
esp32/lib/ESP32-sveltekit/Kinematics.h
+80 -132
View File
@@ -1,55 +1,11 @@
#ifndef Kinematics_h
#define Kinematics_h
#include <cmath>
#include <esp_dsp.h>
#include <cmath>
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;
}
#define RAD2DEG 57.295779513082321 // 180 / PI
#define DEG2RAD 0.017453292519943
typedef struct {
float omega, phi, psi, xm, ym, zm;
@@ -64,10 +20,6 @@ typedef struct {
}
} body_state_t;
#define RAD2DEGREES 57.295779513082321 // 180 / PI
#define DEGREES2RAD 0.017453292519943
class Kinematics
{
private:
@@ -81,17 +33,16 @@ private:
float point[4];
float Q1[4][4];
const float sHp = sin(PI / 2);
const float cHp = cos(PI / 2);
float point_lf[4][4];
public:
public:
float l1, l2, l3, l4;
float L, W;
Kinematics(){
Kinematics() {
l1 = 50;
l2 = 20;
l3 = 120;
@@ -100,7 +51,7 @@ public:
L = 140;
W = 75;
}
~Kinematics(){}
~Kinematics() {}
esp_err_t calculate_inverse_kinematics(body_state_t body_state, float result[12]) {
esp_err_t ret = ESP_OK;
@@ -129,52 +80,47 @@ public:
}
esp_err_t bodyIK(body_state_t p) {
float cos_omega = cos(p.omega*DEGREES2RAD);
float sin_omega = sin(p.omega*DEGREES2RAD);
float cos_phi = cos(p.phi*DEGREES2RAD);
float sin_phi = sin(p.phi*DEGREES2RAD);
float cos_psi = cos(p.psi*DEGREES2RAD);
float sin_psi = sin(p.psi*DEGREES2RAD);
float cos_omega = cos(p.omega * DEG2RAD);
float sin_omega = sin(p.omega * DEG2RAD);
float cos_phi = cos(p.phi * DEG2RAD);
float sin_phi = sin(p.phi * DEG2RAD);
float cos_psi = cos(p.psi * DEG2RAD);
float sin_psi = sin(p.psi * DEG2RAD);
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}
};
{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}};
float point_lf[4][4] = {
{cHp, 0, sHp, L / 2},
{0, 1, 0, 0},
{-sHp, 0, cHp, W / 2},
{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}};
float point_rf[4][4] = {
{cHp, 0, sHp, L / 2},
{0, 1, 0, 0},
{-sHp, 0, cHp, -W / 2},
{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}};
float point_lb[4][4] = {
{cHp, 0, sHp, -L / 2},
{0, 1, 0, 0},
{-sHp, 0, cHp, W / 2},
{0, 0, 0, 1}
};
float point_lb[4][4] = {{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}
};
float point_rb[4][4] = {{cHp, 0, sHp, -L / 2},
{0, 1, 0, 0},
{-sHp, 0, cHp, -W / 2},
{0, 0, 0, 1}};
dspm_mult_f32_ae32((float*) Tm, (float*) point_lf, (float*) Tlf, 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*) Tlb, 4, 4, 4);
dspm_mult_f32_ae32((float*) Tm, (float*) point_rb, (float*) Trb, 4, 4, 4);
dspm_mult_f32_ae32((float*)Tm, (float*)point_lf, (float*)Tlf, 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*)Tlb, 4, 4, 4);
dspm_mult_f32_ae32((float*)Tm, (float*)point_rb, (float*)Trb, 4, 4, 4);
return ESP_OK;
}
@@ -182,7 +128,7 @@ public:
float x = point[0], y = point[1], z = point[2];
float F = sqrt(x * x + y * y - l1 * l1);
if (isnan(F)) F = l1;
F = isnan(F) ? l1 : F;
float G = F - l2;
float H = sqrt(G * G + z * z);
@@ -190,44 +136,46 @@ public:
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;
result[0] = theta1 * RAD2DEG;
result[1] = theta2 * RAD2DEG;
result[2] = theta3 * RAD2DEG;
}
esp_err_t inverse(float a[4][4], float b[4][4]) {
float s[] = {a[0][0] * a[1][1] - a[1][0] * a[0][1],
a[0][0] * a[1][2] - a[1][0] * a[0][2],
a[0][0] * a[1][3] - a[1][0] * a[0][3],
a[0][1] * a[1][2] - a[1][1] * a[0][2],
a[0][1] * a[1][3] - a[1][1] * a[0][3],
a[0][2] * a[1][3] - a[1][2] * a[0][3]};
float c[] = {a[2][0] * a[3][1] - a[3][0] * a[2][1],
a[2][0] * a[3][2] - a[3][0] * a[2][2],
a[2][0] * a[3][3] - a[3][0] * a[2][3],
a[2][1] * a[3][2] - a[3][1] * a[2][2],
a[2][1] * a[3][3] - a[3][1] * a[2][3],
a[2][2] * a[3][3] - a[3][2] * a[2][3]};
float det = s[0] * c[5] - s[1] * c[4] + s[2] * c[3] + s[3] * c[2] -
s[4] * c[1] + s[5] * c[0];
if (det == 0.0) return ESP_FAIL;
float invdet = 1.0 / det;
b[0][0] = (a[1][1] * c[5] - a[1][2] * c[4] + a[1][3] * c[3]) * invdet;
b[0][1] = (-a[0][1] * c[5] + a[0][2] * c[4] - a[0][3] * c[3]) * invdet;
b[0][2] = (a[3][1] * s[5] - a[3][2] * s[4] + a[3][3] * s[3]) * invdet;
b[0][3] = (-a[2][1] * s[5] + a[2][2] * s[4] - a[2][3] * s[3]) * invdet;
b[1][0] = (-a[1][0] * c[5] + a[1][2] * c[2] - a[1][3] * c[1]) * invdet;
b[1][1] = (a[0][0] * c[5] - a[0][2] * c[2] + a[0][3] * c[1]) * invdet;
b[1][2] = (-a[3][0] * s[5] + a[3][2] * s[2] - a[3][3] * s[1]) * invdet;
b[1][3] = (a[2][0] * s[5] - a[2][2] * s[2] + a[2][3] * s[1]) * invdet;
b[2][0] = (a[1][0] * c[4] - a[1][1] * c[2] + a[1][3] * c[0]) * invdet;
b[2][1] = (-a[0][0] * c[4] + a[0][1] * c[2] - a[0][3] * c[0]) * invdet;
b[2][2] = (a[3][0] * s[4] - a[3][1] * s[2] + a[3][3] * s[0]) * invdet;
b[2][3] = (-a[2][0] * s[4] + a[2][1] * s[2] - a[2][3] * s[0]) * invdet;
b[3][0] = (-a[1][0] * c[3] + a[1][1] * c[1] - a[1][2] * c[0]) * invdet;
b[3][1] = (a[0][0] * c[3] - a[0][1] * c[1] + a[0][2] * c[0]) * invdet;
b[3][2] = (-a[3][0] * s[3] + a[3][1] * s[1] - a[3][2] * s[0]) * invdet;
b[3][3] = (a[2][0] * s[3] - a[2][1] * s[1] + a[2][2] * s[0]) * invdet;
return ESP_OK;
}
};
typedef struct {
float x, z, yaw;
} position_target_t;
typedef struct {
float step_length;
float yaw_rate;
float lateral_shift;
float step_velocity;
float swing_period;
float clearance_height;
float penetration_depth;
} gait_state_t;
class GaitPlanner
{
public:
GaitPlanner() { }
~GaitPlanner() { }
void update_trajectory(position_target_t position_target, float dt, body_state_t body_state) {
// for (int8_t i = 0; i < 4; i++) {
// body_state.feet_position[i][0] = position_target.x;
// body_state.feet_position[i][3] = position_target.z;
// }
}
private:
uint16_t time;
uint16_t target_yaw;
};
#endif
esp32/lib/ESP32-sveltekit/MotionService.h
+1 -12
View File
@@ -43,8 +43,7 @@ class MotionService
_socket->onEvent(POSITION_EVENT, [&](JsonObject &root, int originId) { positionEvent(root, originId); });
_socket->onSubscribe(ANGLES_EVENT,
std::bind(&MotionService::syncAngles, this, std::placeholders::_1, std::placeholders::_2));
_socket->onSubscribe(ANGLES_EVENT, std::bind(&MotionService::syncAngles, this, std::placeholders::_1, std::placeholders::_2));
body_state.updateFeet(default_feet_positions);
}
@@ -68,7 +67,6 @@ class MotionService
body_state.xm = array[3];
body_state.ym = array[4];
body_state.zm = array[5];
// syncAngles(String(originId));
}
void handleInput(JsonObject &root, int originId)
@@ -91,11 +89,6 @@ class MotionService
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;
}
}
@@ -135,8 +128,6 @@ class MotionService
break;
}
case MOTION_STATE::WALK:
gaitPlanner.update_trajectory(position_target, MotionInterval, body_state);
kinematics.calculate_inverse_kinematics(body_state, new_angles);
break;
}
@@ -168,14 +159,12 @@ 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 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 default_feet_positions[4][4] = {
esp32/lib/ESP32-sveltekit/WWWData.h
+16644 -16091
View File
File diff suppressed because it is too large Load Diff