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

🧼 Formats components

Browse Source
This commit is contained in:
Rune Harlyk
2024-09-12 22:34:00 +02:00
committed by Rune Harlyk
parent 1990501a66
commit 7849f77712
2 changed files with 315 additions and 278 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/src/lib/components/Visualization.svelte
+146 -109
View File
@@ -1,38 +1,66 @@
<script lang="ts"> <script lang="ts">
import { onDestroy, onMount } from 'svelte'; import { onDestroy, onMount } from 'svelte';
import { BufferGeometry, Line, LineBasicMaterial, Mesh, MeshBasicMaterial, Object3D, SphereGeometry, Vector3, type NormalBufferAttributes, type Object3DEventMap } from 'three'; import {
import { ModesEnum, kinematicData, mode, model, outControllerData, servoAnglesOut, servoAngles, mpu, jointNames } from '$lib/stores'; BufferGeometry,
Line,
LineBasicMaterial,
Mesh,
MeshBasicMaterial,
Object3D,
SphereGeometry,
Vector3,
type NormalBufferAttributes,
type Object3DEventMap
} from 'three';
import {
ModesEnum,
kinematicData,
mode,
model,
outControllerData,
servoAnglesOut,
servoAngles,
mpu,
jointNames
} from '$lib/stores';
import { footColor, populateModelCache, throttler, toeWorldPositions } from '$lib/utilities'; import { footColor, populateModelCache, throttler, toeWorldPositions } from '$lib/utilities';
import SceneBuilder from '$lib/sceneBuilder'; import SceneBuilder from '$lib/sceneBuilder';
import { lerp, degToRad } from 'three/src/math/MathUtils'; import { lerp, degToRad } from 'three/src/math/MathUtils';
import { GUI } from 'three/addons/libs/lil-gui.module.min.js'; import { GUI } from 'three/addons/libs/lil-gui.module.min.js';
import Kinematic, { type body_state_t } from '$lib/kinematic'; import Kinematic, { type body_state_t } from '$lib/kinematic';
import {CalibrationState, EightPhaseWalkState, FourPhaseWalkState, IdleState, RestState, StandState} from '$lib/gait' import {
CalibrationState,
EightPhaseWalkState,
FourPhaseWalkState,
IdleState,
RestState,
StandState
} from '$lib/gait';
import { radToDeg } from 'three/src/math/MathUtils.js'; import { radToDeg } from 'three/src/math/MathUtils.js';
import type { URDFRobot } from 'urdf-loader'; import type { URDFRobot } from 'urdf-loader';
import { get } from 'svelte/store'; import { get } from 'svelte/store';
export let sky = true export let sky = true;
export let orbit = false export let orbit = false;
export let panel = true export let panel = true;
export let debug = false export let debug = false;
export let ground = true export let ground = true;
let sceneManager = new SceneBuilder(); let sceneManager = new SceneBuilder();
let canvas: HTMLCanvasElement let canvas: HTMLCanvasElement;
let currentModelAngles: number[] = new Array(12).fill(0); let currentModelAngles: number[] = new Array(12).fill(0);
let modelTargetAngles: number[] = new Array(12).fill(0) let modelTargetAngles: number[] = new Array(12).fill(0);
let gui_panel: GUI let gui_panel: GUI;
let Throttler = new throttler() let Throttler = new throttler();
let feet_trace = new Array(4).fill([]); let feet_trace = new Array(4).fill([]);
let trace_lines: BufferGeometry<NormalBufferAttributes>[] = [] let trace_lines: BufferGeometry<NormalBufferAttributes>[] = [];
let target: Object3D<Object3DEventMap>; let target: Object3D<Object3DEventMap>;
let target_position = {x: 0, z: 0, yaw: 0} let target_position = { x: 0, z: 0, yaw: 0 };
let kinematic = new Kinematic() let kinematic = new Kinematic();
let planners = { let planners = {
[ModesEnum.Deactivated]: new IdleState(), [ModesEnum.Deactivated]: new IdleState(),
@@ -42,10 +70,10 @@
[ModesEnum.Stand]: new StandState(), [ModesEnum.Stand]: new StandState(),
[ModesEnum.Crawl]: new EightPhaseWalkState(), [ModesEnum.Crawl]: new EightPhaseWalkState(),
[ModesEnum.Walk]: new FourPhaseWalkState() [ModesEnum.Walk]: new FourPhaseWalkState()
} };
let lastTick = performance.now() let lastTick = performance.now();
const dir = [1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1] const dir = [1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1];
let body_state = { let body_state = {
omega: 0, omega: 0,
@@ -55,10 +83,10 @@
ym: 0.5, ym: 0.5,
zm: 0, zm: 0,
feet: planners[ModesEnum.Idle].default_feet_pos feet: planners[ModesEnum.Idle].default_feet_pos
} };
let settings = { let settings = {
'Internal kinematic':false, 'Internal kinematic': true,
'Robot transform controls': false, 'Robot transform controls': false,
'Auto orient robot': true, 'Auto orient robot': true,
'Trace feet': debug, 'Trace feet': debug,
@@ -66,32 +94,32 @@
'Trace points': 30, 'Trace points': 30,
'Fix camera on robot': true, 'Fix camera on robot': true,
'Smooth motion': true, 'Smooth motion': true,
'omega': 0, omega: 0,
'phi': 0, phi: 0,
'psi': 0, psi: 0,
'xm': 0, xm: 0,
'ym': 0.7, ym: 0.7,
'zm': 0, zm: 0,
'Background': "black" Background: 'black'
} };
onMount(async () => { onMount(async () => {
await populateModelCache(); await populateModelCache();
await createScene(); await createScene();
servoAngles.subscribe(updateAnglesFromStore) servoAngles.subscribe(updateAnglesFromStore);
if (!panel) createPanel(); if (panel) createPanel();
}); });
onDestroy(() => { onDestroy(() => {
canvas.remove() canvas.remove();
gui_panel?.destroy() gui_panel?.destroy();
}); });
const updateAnglesFromStore = (angles: number[]) => { const updateAnglesFromStore = (angles: number[]) => {
if (sceneManager.isDragging) return if (sceneManager.isDragging) return;
if (settings['Internal kinematic']) return if (settings['Internal kinematic']) return;
modelTargetAngles = angles; modelTargetAngles = angles;
} };
const createPanel = () => { const createPanel = () => {
gui_panel = new GUI({ width: 310 }); gui_panel = new GUI({ width: 310 });
@@ -99,25 +127,25 @@
gui_panel.domElement.id = 'three-gui-panel'; gui_panel.domElement.id = 'three-gui-panel';
const general = gui_panel.addFolder('General'); const general = gui_panel.addFolder('General');
general.add(settings, 'Internal kinematic') general.add(settings, 'Internal kinematic');
general.add(settings, 'Robot transform controls') general.add(settings, 'Robot transform controls');
general.add(settings, 'Auto orient robot') general.add(settings, 'Auto orient robot');
const kinematic = gui_panel.addFolder('Kinematics'); const kinematic = gui_panel.addFolder('Kinematics');
kinematic.add(settings, 'omega', -20, 20).onChange(updateKinematicPosition).listen(); kinematic.add(settings, 'omega', -20, 20).onChange(updateKinematicPosition).listen();
kinematic.add(settings, 'phi', -30, 30).onChange(updateKinematicPosition).listen(); kinematic.add(settings, 'phi', -30, 30).onChange(updateKinematicPosition).listen();
kinematic.add(settings, 'psi', -20, 15).onChange(updateKinematicPosition).listen(); kinematic.add(settings, 'psi', -20, 15).onChange(updateKinematicPosition).listen();
kinematic.add(settings, 'xm', -1, 1).onChange(updateKinematicPosition).listen() kinematic.add(settings, 'xm', -1, 1).onChange(updateKinematicPosition).listen();
kinematic.add(settings, 'ym', 0, 1).onChange(updateKinematicPosition).listen() kinematic.add(settings, 'ym', 0, 1).onChange(updateKinematicPosition).listen();
kinematic.add(settings, 'zm', -1.3, 1.3).onChange(updateKinematicPosition).listen() kinematic.add(settings, 'zm', -1.3, 1.3).onChange(updateKinematicPosition).listen();
const visibility = gui_panel.addFolder('Visualization'); const visibility = gui_panel.addFolder('Visualization');
visibility.add(settings, 'Trace feet') visibility.add(settings, 'Trace feet');
visibility.add(settings, 'Trace points', 1, 1000, 1) visibility.add(settings, 'Trace points', 1, 1000, 1);
visibility.add(settings, 'Target position') visibility.add(settings, 'Target position');
visibility.add(settings, 'Smooth motion') visibility.add(settings, 'Smooth motion');
visibility.addColor(settings, 'Background') visibility.addColor(settings, 'Background');
} };
const updateKinematicPosition = () => { const updateKinematicPosition = () => {
kinematicData.set([ kinematicData.set([
@@ -127,12 +155,15 @@
settings.xm, settings.xm,
settings.ym, settings.ym,
settings.zm settings.zm
]) ]);
} };
const updateAngles = (name: string, angle: number) => { const updateAngles = (name: string, angle: number) => {
modelTargetAngles[$jointNames.indexOf(name)] = angle * (180 / Math.PI); modelTargetAngles[$jointNames.indexOf(name)] = angle * (180 / Math.PI);
Throttler.throttle(() => servoAnglesOut.set(modelTargetAngles.map(num => Math.round(num))), 100) Throttler.throttle(
() => servoAnglesOut.set(modelTargetAngles.map(num => Math.round(num))),
100
);
}; };
const createScene = async () => { const createScene = async () => {
@@ -149,8 +180,7 @@
.addRenderCb(render) .addRenderCb(render)
.startRenderLoop(); .startRenderLoop();
if (ground) sceneManager if (ground) sceneManager.addGroundPlane();
.addGroundPlane()
const geometry = new SphereGeometry(0.1, 32, 16); const geometry = new SphereGeometry(0.1, 32, 16);
const material = new MeshBasicMaterial({ color: 0xffff00 }); const material = new MeshBasicMaterial({ color: 0xffff00 });
@@ -158,9 +188,9 @@
sceneManager.scene.add(target); sceneManager.scene.add(target);
if (debug) { if (debug) {
sceneManager.addDragControl(updateAngles) sceneManager.addDragControl(updateAngles);
} }
if (sky) sceneManager.addSky() if (sky) sceneManager.addSky();
for (let i = 0; i < 4; i++) { for (let i = 0; i < 4; i++) {
const geometry = new BufferGeometry(); const geometry = new BufferGeometry();
@@ -173,21 +203,21 @@
const renderTraceLines = (foot_positions: Vector3[]) => { const renderTraceLines = (foot_positions: Vector3[]) => {
if (!settings['Trace feet']) { if (!settings['Trace feet']) {
if (!feet_trace.length) return if (!feet_trace.length) return;
trace_lines.forEach((line, i) => line.setFromPoints(feet_trace[i].slice(-1))) trace_lines.forEach((line, i) => line.setFromPoints(feet_trace[i].slice(-1)));
feet_trace = new Array(4).fill([]) feet_trace = new Array(4).fill([]);
return return;
} }
trace_lines.forEach((line, i) => { trace_lines.forEach((line, i) => {
feet_trace[i].push(foot_positions[i]) feet_trace[i].push(foot_positions[i]);
feet_trace[i] = feet_trace[i].slice(-settings['Trace points']) feet_trace[i] = feet_trace[i].slice(-settings['Trace points']);
line.setFromPoints(feet_trace[i]); line.setFromPoints(feet_trace[i]);
}) });
} };
const calculate_kinematics = () => { const calculate_kinematics = () => {
if (sceneManager.isDragging || !settings['Internal kinematic']) return if (sceneManager.isDragging || !settings['Internal kinematic']) return;
const position: body_state_t = { const position: body_state_t = {
omega: settings.omega, omega: settings.omega,
phi: settings.phi, phi: settings.phi,
@@ -196,36 +226,40 @@
ym: settings.ym, ym: settings.ym,
zm: settings.zm, zm: settings.zm,
feet: body_state.feet feet: body_state.feet
} };
let new_angles = kinematic.calcIK(position).map((x, i) => radToDeg(x * dir[i])); let new_angles = kinematic.calcIK(position).map((x, i) => radToDeg(x * dir[i]));
modelTargetAngles = new_angles; modelTargetAngles = new_angles;
} };
const orient_robot = (robot: URDFRobot, toes: Vector3[]) => { const orient_robot = (robot: URDFRobot, toes: Vector3[]) => {
if (settings['Robot transform controls'] || !settings['Auto orient robot']) return if (settings['Robot transform controls'] || !settings['Auto orient robot']) return;
robot.position.y = robot.position.y - Math.min(...toes.map(toe => toe.y)); robot.position.y = robot.position.y - Math.min(...toes.map(toe => toe.y));
robot.position.z = smooth(robot.position.z, -settings.xm, 0.1); robot.position.z = smooth(robot.position.z, -settings.xm, 0.1);
robot.position.x = smooth(robot.position.x, -settings.zm, 0.1); robot.position.x = smooth(robot.position.x, -settings.zm, 0.1);
robot.rotation.z = smooth(robot.rotation.z, degToRad(-settings.phi + $mpu.heading + 90), 0.1); robot.rotation.z = smooth(
robot.rotation.z,
degToRad(-settings.phi + $mpu.heading + 90),
0.1
);
robot.rotation.y = smooth(robot.rotation.y, degToRad(settings.omega), 0.1); robot.rotation.y = smooth(robot.rotation.y, degToRad(settings.omega), 0.1);
robot.rotation.x = smooth(robot.rotation.x, degToRad(settings.psi - 90), 0.1); robot.rotation.x = smooth(robot.rotation.x, degToRad(settings.psi - 90), 0.1);
} };
const update_camera = (robot: URDFRobot) => { const update_camera = (robot: URDFRobot) => {
if (!settings['Fix camera on robot']) return if (!settings['Fix camera on robot']) return;
sceneManager.orbit.target = robot.position.clone() sceneManager.orbit.target = robot.position.clone();
} };
const smooth = (start: number, end: number, amount: number) => { const smooth = (start: number, end: number, amount: number) => {
return settings['Smooth motion'] ? lerp(start, end, amount) : end return settings['Smooth motion'] ? lerp(start, end, amount) : end;
} };
const update_gait = () => { const update_gait = () => {
if (sceneManager.isDragging || !settings['Internal kinematic']) return if (sceneManager.isDragging || !settings['Internal kinematic']) return;
const controlData = get(outControllerData) const controlData = get(outControllerData);
const data = { const data = {
stop: controlData[0], stop: controlData[0],
lx: controlData[1], lx: controlData[1],
@@ -233,64 +267,67 @@
rx: controlData[3], rx: controlData[3],
ry: controlData[4], ry: controlData[4],
h: controlData[5], h: controlData[5],
s: controlData[6], s: controlData[6]
}; };
body_state.ym = ((data.h + 127) * 0.75) / 100; body_state.ym = ((data.h + 127) * 0.75) / 100;
let planner = planners[get(mode)] let planner = planners[get(mode)];
const delta = performance.now() - lastTick const delta = performance.now() - lastTick;
lastTick = performance.now() lastTick = performance.now();
body_state = planner.step(body_state, data, delta); body_state = planner.step(body_state, data, delta);
settings.omega = body_state.omega settings.omega = body_state.omega;
settings.phi = body_state.phi settings.phi = body_state.phi;
settings.psi = body_state.psi settings.psi = body_state.psi;
settings.xm = body_state.xm settings.xm = body_state.xm;
settings.ym = body_state.ym settings.ym = body_state.ym;
settings.zm = body_state.zm settings.zm = body_state.zm;
} };
const update_robot_position = (robot: URDFRobot) => { const update_robot_position = (robot: URDFRobot) => {
if (!settings['Robot transform controls']) return if (!settings['Robot transform controls']) return;
settings.omega = radToDeg(robot.rotation.y) settings.omega = radToDeg(robot.rotation.y);
settings.phi = radToDeg(robot.rotation.z) + $mpu.heading -90 settings.phi = radToDeg(robot.rotation.z) + $mpu.heading - 90;
settings.psi = radToDeg(robot.rotation.x) + 90 settings.psi = radToDeg(robot.rotation.x) + 90;
settings.xm = robot.position.z * 100 settings.xm = robot.position.z * 100;
settings.zm = -robot.position.x * 100 settings.zm = -robot.position.x * 100;
} };
const updateTargetPosition = () => { const updateTargetPosition = () => {
target.visible = settings['Target position'] target.visible = settings['Target position'];
target.position.x = smooth(target.position.x, target_position.x, 0.5) target.position.x = smooth(target.position.x, target_position.x, 0.5);
target.position.z = smooth(target.position.z, target_position.z, 0.5) target.position.z = smooth(target.position.z, target_position.z, 0.5);
} };
const render = () => { const render = () => {
const robot = sceneManager.model; const robot = sceneManager.model;
if (!robot) return; if (!robot) return;
const toes = toeWorldPositions(robot) const toes = toeWorldPositions(robot);
renderTraceLines(toes) renderTraceLines(toes);
update_camera(robot) update_camera(robot);
update_gait() update_gait();
calculate_kinematics() calculate_kinematics();
update_robot_position(robot) update_robot_position(robot);
sceneManager.transformControl.showX = settings['Robot transform controls'] sceneManager.transformControl.showX = settings['Robot transform controls'];
sceneManager.transformControl.showY = settings['Robot transform controls'] sceneManager.transformControl.showY = settings['Robot transform controls'];
sceneManager.transformControl.showZ = settings['Robot transform controls'] sceneManager.transformControl.showZ = settings['Robot transform controls'];
for (let i = 0; i < $jointNames.length; i++) { for (let i = 0; i < $jointNames.length; i++) {
currentModelAngles[i] = smooth((robot.joints[$jointNames[i]].angle as number) * (180 / Math.PI), modelTargetAngles[i], 0.1); currentModelAngles[i] = smooth(
(robot.joints[$jointNames[i]].angle as number) * (180 / Math.PI),
modelTargetAngles[i],
0.1
);
robot.joints[$jointNames[i]].setJointValue(degToRad(currentModelAngles[i])); robot.joints[$jointNames[i]].setJointValue(degToRad(currentModelAngles[i]));
} }
orient_robot(robot, toes) orient_robot(robot, toes);
updateTargetPosition(); updateTargetPosition();
}; };
</script> </script>
<svelte:window on:resize={sceneManager.fillParent} /> <svelte:window on:resize={sceneManager.fillParent} />
app/src/routes/+layout.svelte
+10 -10
View File
@@ -39,10 +39,10 @@
addEventListeners(); addEventListeners();
outControllerData.subscribe((data) => socket.sendEvent('input', { data })); outControllerData.subscribe(data => socket.sendEvent('input', { data }));
mode.subscribe((data) => socket.sendEvent('mode', { data })); mode.subscribe(data => socket.sendEvent('mode', { data }));
servoAnglesOut.subscribe((data) => socket.sendEvent('angles', { data })); servoAnglesOut.subscribe(data => socket.sendEvent('angles', { data }));
kinematicData.subscribe((data) => socket.sendEvent('position', { data })); kinematicData.subscribe(data => socket.sendEvent('position', { data }));
}); });
onDestroy(() => { onDestroy(() => {
@@ -55,15 +55,15 @@
socket.on('error', handleError); socket.on('error', handleError);
socket.on('rssi', handleNetworkStatus); socket.on('rssi', handleNetworkStatus);
socket.on('mode', (data: ModesEnum) => mode.set(data)); socket.on('mode', (data: ModesEnum) => mode.set(data));
socket.on('analytics', handleAnalytics);
socket.on('angles', (angles: number[]) => { socket.on('angles', (angles: number[]) => {
if (angles.length) servoAngles.set(angles); if (angles.length) servoAngles.set(angles);
}); });
features.subscribe(data => { features.subscribe(data => {
if (data.analytics) socket.on('analytics', handleAnalytics); if (data?.battery) socket.on('battery', handleBattery);
if (data.battery) socket.on('battery', handleBattery); if (data?.download_firmware) socket.on('otastatus', handleOAT);
if (data.download_firmware) socket.on('otastatus', handleOAT); if (data?.sonar) socket.on('sonar', data => console.log(data));
if (data.sonar) socket.on('sonar', (data) => console.log(data)); });
})
}; };
const removeEventListeners = () => { const removeEventListeners = () => {
@@ -109,7 +109,7 @@
<title>{$page.data.title}</title> <title>{$page.data.title}</title>
</svelte:head> </svelte:head>
{#if $features.security && $user.bearer_token === ''} {#if $features?.security && $user.bearer_token === ''}
<Login /> <Login />
{:else} {:else}
<div class="drawer"> <div class="drawer">