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

🏍️ Adds position event for updating kinematics

Browse Source
This commit is contained in:
Rune Harlyk
2024-05-23 17:29:38 +02:00
committed by Rune Harlyk
parent addf57b2a6
commit eb609e9873
4 changed files with 84 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/src/lib/components/Visualization.svelte
+24 -10
View File
@@ -2,7 +2,7 @@
import { onDestroy, onMount } from 'svelte';
import { BufferGeometry, Line, LineBasicMaterial, Vector3, type NormalBufferAttributes } from 'three';
import uzip from 'uzip';
import { model, outControllerData, servoAnglesOut } from '$lib/stores';
import { kinematicData, 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';
@@ -38,9 +38,8 @@
[-100, -100, -100, 1],
];
let settings = {
'Internal kinematic':true,
'Internal kinematic':false,
'Trace feet':debug,
'Trace points': 30,
'Fix camera on robot': true,
@@ -99,18 +98,29 @@
general.add(settings, 'Internal kinematic')
const kinematic = gui_panel.addFolder('Kinematics');
kinematic.add(settings, 'omega', -20, 20)
kinematic.add(settings, 'phi', -30, 30)
kinematic.add(settings, 'psi', -20, 15)
kinematic.add(settings, 'x', -90, 90)
kinematic.add(settings, 'y', 0, 200)
kinematic.add(settings, 'z', -130, 130)
kinematic.add(settings, 'omega', -20, 20).onChange(updateKinematicPosition)
kinematic.add(settings, 'phi', -30, 30).onChange(updateKinematicPosition)
kinematic.add(settings, 'psi', -20, 15).onChange(updateKinematicPosition)
kinematic.add(settings, 'x', -90, 90).onChange(updateKinematicPosition)
kinematic.add(settings, 'y', 0, 200).onChange(updateKinematicPosition)
kinematic.add(settings, 'z', -130, 130).onChange(updateKinematicPosition)
const visibility = gui_panel.addFolder('Visualization');
visibility.add(settings, 'Trace feet')
visibility.add(settings, 'Trace points', 1, 1000, 1)
}
const updateKinematicPosition = () => {
kinematicData.set([
settings.omega,
settings.phi,
settings.psi,
settings.x,
settings.y,
settings.z
])
}
const cacheModelFiles = async () => {
let data = await fetch('/stl.zip').then((data) => data.arrayBuffer());
@@ -201,7 +211,11 @@
if (settings['Internal kinematic']) calculate_kinematics()
robot.position.y = robot.position.y - Math.min(...toes.map(toe => toe.y));
robot.rotation.z = lerp(robot.rotation.z, degToRad($mpu.heading + 90), 0.1);
robot.position.z = lerp(robot.position.z, -settings.x / 100, 0.1);
robot.rotation.z = lerp(robot.rotation.z, degToRad(settings.phi + $mpu.heading + 90), 0.1);
robot.rotation.y = lerp(robot.rotation.y, degToRad(settings.omega - settings.z / 2.5), 0.1);
robot.rotation.x = lerp(robot.rotation.x, degToRad(settings.psi - 90), 0.1);
for (let i = 0; i < $jointNames.length; i++) {
currentModelAngles[i] = lerp(
app/src/lib/stores/model-store.ts
+2
View File
@@ -23,6 +23,8 @@ export const mode: Writable<ModesEnum> = writable(ModesEnum.Idle);
export const outControllerData = writable([0, 0, 0, 0, 0, 70, 0]);
export const kinematicData = writable([0, 0, 0, 0, 100, 0]);
export const input: Writable<ControllerInput> = writable({
left: { x: 0, y: 0 },
right: { x: 0, y: 0 },
app/src/routes/+layout.svelte
+2 -1
View File
@@ -13,7 +13,7 @@
import Menu from './menu.svelte';
import Statusbar from './statusbar.svelte';
import Login from './login.svelte';
import { ModesEnum, mode, outControllerData, servoAngles, servoAnglesOut, socket } from '$lib/stores';
import { ModesEnum, kinematicData, mode, outControllerData, servoAngles, servoAnglesOut, socket } from '$lib/stores';
import type { Analytics, Battery, DownloadOTA } from '$lib/types/models';
import { api } from '$lib/api';
@@ -29,6 +29,7 @@
outControllerData.subscribe((data) => socket.sendEvent("input", {data}));
mode.subscribe((data) => socket.sendEvent("mode", {data}));
servoAnglesOut.subscribe((data) => socket.sendEvent("angles", {data}));
kinematicData.subscribe((data) => socket.sendEvent("position", {data}));
});
onDestroy(() => {
esp32/lib/ESP32-sveltekit/MotionService.h
+56 -30
View File
@@ -9,6 +9,7 @@
#define LIGHT_SETTINGS_ENDPOINT_PATH "/api/input"
#define ANGLES_EVENT "angles"
#define INPUT_EVENT "input"
#define POSITION_EVENT "position"
#define MODE_EVENT "mode"
enum class MOTION_STATE
@@ -32,12 +33,16 @@ class MotionService
_socket->registerEvent(INPUT_EVENT);
_socket->registerEvent(ANGLES_EVENT);
_socket->registerEvent(MODE_EVENT);
_socket->registerEvent(POSITION_EVENT);
_socket->onEvent(INPUT_EVENT, [&](JsonObject &root, int originId) { handleInput(root, originId); });
_socket->onEvent(MODE_EVENT, [&](JsonObject &root, int originId) { handleMode(root, originId); });
_socket->onEvent(ANGLES_EVENT, [&](JsonObject &root, int originId) { anglesEvent(root, originId); });
_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));
}
@@ -52,6 +57,18 @@ class MotionService
syncAngles(String(originId));
}
void positionEvent(JsonObject &root, int originId)
{
JsonArray array = root["data"].as<JsonArray>();
position.omega = array[0];
position.phi = array[1];
position.psi = array[2];
position.xm = array[3];
position.ym = array[4];
position.zm = array[5];
// syncAngles(String(originId));
}
void handleInput(JsonObject &root, int originId)
{
JsonArray array = root["data"].as<JsonArray>();
@@ -59,6 +76,20 @@ class MotionService
{
input[i] = array[i];
}
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 = {
0,
rx / 4,
ry / 4,
ly / 2,
(h + 128) * (float)0.7,
lx / 2
};
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]);
}
@@ -76,7 +107,6 @@ class MotionService
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());
}
float lerp(float start, float end, float t) {
@@ -99,31 +129,8 @@ class MotionService
break;
case MOTION_STATE::STAND: {
float lp[4][4] = {
{ 100, -100, 100, 1},
{ 100, -100, -100, 1},
{-100, -100, 100, 1},
{-100, -100, -100, 1}
};
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);
kinematics.calculate_inverse_kinematics(lp, position, new_angles);
for (int i = 0; i < 12; i++) {
float new_angle = lerp(angles[i], new_angles[i] * dir[i], 0.3);
@@ -138,10 +145,20 @@ class MotionService
break;
}
case MOTION_STATE::WALK:
angles[1] += dir;
if (angles[1] >= 90) dir = -1;
if (angles[1] <= 0) dir = 1;
updated = true;
float new_angles[12] = {0,};
lp[0][1] += walk_dir;
if (lp[0][1] >= 200) walk_dir = -1;
if (lp[0][1] <= 100) walk_dir = 1;
kinematics.calculate_inverse_kinematics(lp, position, new_angles);
for (int i = 0; i < 12; i++) {
float new_angle = lerp(angles[i], new_angles[i] * dir[i], 0.3);
if (new_angle != angles[i]) {
angles[i] = new_angle;
updated = true;
}
}
break;
}
return updated;
@@ -163,13 +180,22 @@ class MotionService
constexpr static int MotionInterval = 100;
position_t position = {0, 0, 0, 0, 0, 0};
float dir[12] = {-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1};
float lp[4][4] = {
{ 100, -100, 100, 1},
{ 100, -100, -100, 1},
{-100, -100, 100, 1},
{-100, -100, -100, 1}
};
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;
int walk_dir = 2;
};
#endif