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

✨ Refactors and adds new mode concept

Browse Source
This commit is contained in:
Rune Harlyk
2024-02-24 01:50:05 +01:00
committed by Rune Harlyk
parent e6296555da
commit 0f676e3543
20 changed files with 244 additions and 177 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.vscode/settings.json
Vendored
+13 -12
View File
@@ -6,17 +6,18 @@
"editor.detectIndentation": false,
"cmake.sourceDirectory": "C:/data/repos/Hardware/Spot Micro - Leika/.pio/libdeps/esp32cam/esp32-camera",
"cSpell.words": [
"Adafruit",
"IRAM",
"Leika",
"lerp",
"MDNS",
"Psram",
"smnc",
"ssid",
"URDF",
"uzip",
"WEBSERVER",
"xacro"
"Adafruit",
"IRAM",
"Leika",
"lerp",
"MDNS",
"nipplejs",
"Psram",
"smnc",
"ssid",
"URDF",
"uzip",
"WEBSERVER",
"xacro"
]
}
app/src/App.svelte
+4 -3
View File
@@ -6,15 +6,16 @@
import Controller from './routes/Controller.svelte';
import { fileService } from '$lib/services';
import Settings from './routes/Settings.svelte';
import { jointNames, model, outControllerData } from '$lib/store';
import { loadModelAsync } from '$lib/utilities';
import { socketLocation } from '$lib/utilities';
import { jointNames, model, outControllerData, mode } from '$lib/stores';
import { loadModelAsync, socketLocation } from '$lib/utilities';
import type { Result } from '$lib/utilities/result';
export let url = window.location.pathname;
onMount(async () => {
socketService.connect(socketLocation);
socketService.addPublisher(outControllerData);
socketService.addPublisher(mode, 'mode');
registerFetchIntercept();
const modelRes = await loadModelAsync('/spot_micro.urdf.xacro');
app/src/components/Controls.svelte
+41 -38
View File
@@ -1,18 +1,15 @@
<script lang="ts">
import nipplejs from 'nipplejs';
import { onMount } from 'svelte';
import { throttler, toInt8 } from '$lib/utilities';
import socketService from '$lib/services/socket-service';
import { emulateModel, input, outControllerData } from '$lib/store';
import { capitalize, throttler, toInt8 } from '$lib/utilities';
import { input, outControllerData, mode, modes } from '$lib/stores';
import type { vector } from '$lib/models';
let throttle = new throttler();
let left: nipplejs.JoystickManager;
let right: nipplejs.JoystickManager;
let throttle_timing = 40;
let mode = 'rest'; // 'rest' | 'stand' | 'stand+' | 'walk'
let data = new Int8Array(6);
onMount(() => {
@@ -24,22 +21,6 @@
restOpacity: 0.3
});
left.on('move', (evt, data) => {
input.update((o) => {
o.left = data.vector;
return o;
});
throttle.throttle(updateData, throttle_timing);
});
left.on('end', (evt, data) => {
input.update((o) => {
o.left = { x: 0, y: 0 };
return o;
});
throttle.throttle(updateData, throttle_timing);
});
right = nipplejs.create({
zone: document.getElementById('right') as HTMLElement,
color: 'grey',
@@ -48,23 +29,20 @@
restOpacity: 0.3
});
right.on('move', (evt, data) => {
input.update((o) => {
o.right = data.vector;
return o;
});
throttle.throttle(updateData, throttle_timing);
});
right.on('end', (evt, data) => {
input.update((o) => {
o.right = { x: 0, y: 0 };
return o;
});
throttle.throttle(updateData, throttle_timing);
});
left.on('move', (_, data) => handleJoyMove('left', data.vector));
left.on('end', (_, __) => handleJoyMove('left', { x: 0, y: 0 }));
right.on('move', (_, data) => handleJoyMove('right', data.vector));
right.on('end', (_, __) => handleJoyMove('right', { x: 0, y: 0 }));
});
const handleJoyMove = (key: 'left' | 'right', data: vector) => {
input.update((inputData) => {
inputData[key] = data;
return inputData;
});
throttle.throttle(updateData, throttle_timing);
};
const updateData = () => {
data[0] = 0;
data[1] = toInt8($input.left.x, -1, 1);
@@ -75,8 +53,22 @@
data[6] = toInt8($input.speed, 0, 100);
outControllerData.set(data);
};
if (!$emulateModel) socketService.send(data);
const handleKeyup = (event: KeyboardEvent) => {
const down = event.type === 'keydown';
input.update((data) => {
if (event.key === 'w') data.left.y = down ? -1 : 0;
if (event.key === 'a') data.left.x = down ? -1 : 0;
if (event.key === 's') data.left.y = down ? 1 : 0;
if (event.key === 'd') data.left.x = down ? 1 : 0;
return data;
});
throttle.throttle(updateData, throttle_timing);
};
const changeMode = (modeValue: Modes) => {
mode.set(modeValue);
};
</script>
@@ -86,4 +78,15 @@
<div class="flex-1" />
<div id="right" class="flex w-60 items-center" />
</div>
<div class="absolute bottom-0 z-10 p-4 gap-4 flex">
{#each modes as modeValue}
<button
on:click={() => changeMode(modeValue)}
class="rounded-md outline outline-2 text-zinc-200 outline-zinc-600 p-2">
{capitalize(modeValue)}
</button>
{/each}
</div>
</div>
<svelte:window on:keyup={handleKeyup} on:keydown={handleKeyup} />
app/src/components/Topbar.svelte
+2 -2
View File
@@ -1,8 +1,9 @@
<script lang="ts">
import socketService from '$lib/services/socket-service';
import { Icon, Bars3, XMark, Power, Battery100, Signal, SignalSlash } from 'svelte-hero-icons';
import { emulateModel } from '$lib/store';
import { emulateModel } from '$lib/stores';
import { Link, useLocation } from 'svelte-routing';
import { isConnected } from '$lib/stores';
const views = ['Virtual environment', 'Robot camera'];
const modes = ['Drive', 'Choreography'];
@@ -12,7 +13,6 @@
let selected_view = views[0];
let selected_modes = modes[0];
let settingOpen = window.location.pathname.includes('/settings');
let isConnected = socketService.isConnected;
$: emulateModel.set(selected_view === views[0]);
$: settingOpen = $location.pathname.includes('/settings');
app/src/components/Views/Model.svelte
+9 -46
View File
@@ -3,11 +3,11 @@
import { CanvasTexture, CircleGeometry, Mesh, MeshBasicMaterial } from 'three';
import socketService from '$lib/services/socket-service';
import uzip from 'uzip';
import { model } from '$lib/store';
import { model } from '$lib/stores';
import { ForwardKinematics } from '$lib/kinematic';
import { location } from '$lib/utilities';
import { fileService } from '$lib/services';
import { servoAngles, mpu } from '$lib/stores';
import { servoAngles, mpu, jointNames } from '$lib/stores';
import SceneBuilder from '$lib/sceneBuilder';
import { lerp, degToRad } from 'three/src/math/MathUtils';
@@ -18,34 +18,9 @@
let modelAngles: number[] | Int16Array = new Array(12).fill(0);
let modelTargetAngles: number[] | Int16Array = new Array(12).fill(0);
let modelBodyAngles: EulerAngle = { omega: 0, phi: 0, psi: 0 };
let modelTargeBodyAngles: EulerAngle = { omega: 0, phi: 0, psi: 0 };
const videoStream = `//${location}/api/stream`;
let showModel = true,
showStream = false;
const servoNames = [
'front_left_shoulder',
'front_left_leg',
'front_left_foot',
'front_right_shoulder',
'front_right_leg',
'front_right_foot',
'rear_left_shoulder',
'rear_left_leg',
'rear_left_foot',
'rear_right_shoulder',
'rear_right_leg',
'rear_right_foot'
];
interface EulerAngle {
omega: number;
phi: number;
psi: number;
}
let showStream = false;
onMount(async () => {
await cacheModelFiles();
@@ -68,12 +43,12 @@
};
const updateAngles = (name: string, angle: number) => {
modelTargetAngles[servoNames.indexOf(name)] = angle * (180 / Math.PI);
modelTargetAngles[$jointNames.indexOf(name)] = angle * (180 / Math.PI);
socketService.send(
JSON.stringify({
type: 'kinematic/angle',
angle: angle * (180 / Math.PI),
id: servoNames.indexOf(name)
id: $jointNames.indexOf(name)
})
);
};
@@ -100,7 +75,7 @@
};
const addVideoStream = () => {
context = streamCanvas.getContext('2d');
context = streamCanvas.getContext('2d')!;
texture = new CanvasTexture(stream);
const liveStream = new Mesh(
new CircleGeometry(35, 32),
@@ -132,26 +107,14 @@
handleVideoStream();
for (let i = 0; i < servoNames.length; i++) {
for (let i = 0; i < $jointNames.length; i++) {
modelAngles[i] = lerp(
robot.joints[servoNames[i]].angle * (180 / Math.PI),
(robot.joints[$jointNames[i]].angle as number) * (180 / Math.PI),
modelTargetAngles[i],
0.1
);
robot.joints[servoNames[i]].setJointValue(degToRad(modelAngles[i]));
robot.joints[$jointNames[i]].setJointValue(degToRad(modelAngles[i]));
}
modelBodyAngles.omega = lerp(
robot.rotation.x * (180 / Math.PI),
modelTargeBodyAngles.omega - 90,
0.1
);
modelBodyAngles.phi = lerp(robot.rotation.y * (180 / Math.PI), modelTargeBodyAngles.phi, 0.1);
modelBodyAngles.psi = lerp(
robot.rotation.z * (180 / Math.PI),
modelTargeBodyAngles.psi + 90,
0.1
);
};
</script>
app/src/components/settings/Calibration.svelte
+7 -5
View File
@@ -1,6 +1,6 @@
<script lang="ts">
import { onMount } from 'svelte';
import { jointNames } from '../../lib/store';
import { jointNames } from '../../lib/stores';
type Servo = {
id: number;
@@ -10,7 +10,7 @@
pwmFor180: number;
};
let servos: any[] = [];
let servos: Servo[] = [];
onMount(() => {
jointNames.subscribe((data) => {
@@ -58,7 +58,8 @@
id="minPWM"
class="bg-zinc-800"
value={servos[selectedServo].minPWM}
on:blur={(event) => updateServoValue(selectedServo, 'minPWM', Number(event.target.value))}
on:blur={(event) =>
updateServoValue(selectedServo ?? 0, 'minPWM', Number(event.target?.value))}
/>
<label for="maxPWM">Max PWM:</label>
@@ -67,7 +68,8 @@
id="maxPWM"
class="bg-zinc-800"
value={servos[selectedServo].maxPWM}
on:blur={(event) => updateServoValue(selectedServo, 'maxPWM', Number(event.target.value))}
on:blur={(event) =>
updateServoValue(selectedServo ?? 0, 'maxPWM', Number(event.target?.value))}
/>
<label for="pwmFor180">PWM for 180°:</label>
@@ -77,7 +79,7 @@
class="bg-zinc-800"
value={servos[selectedServo].pwmFor180}
on:blur={(event) =>
updateServoValue(selectedServo, 'pwmFor180', Number(event.target.value))}
updateServoValue(selectedServo ?? 0, 'pwmFor180', Number(event.target?.value))}
/>
</div>
{/if}
app/src/components/settings/Configuration.svelte
+2 -4
View File
@@ -1,10 +1,8 @@
<script lang="ts">
import socketService from '$lib/services/socket-service';
import { socketService } from '$lib/services';
import { isConnected, settings } from '$lib/stores';
import { onMount } from 'svelte';
let isConnected = socketService.isConnected;
let settings = socketService.settings;
onMount(() => {
if ($isConnected) {
const message = JSON.stringify({ type: 'system/settings' });
app/src/components/settings/Info.svelte
+1 -4
View File
@@ -2,10 +2,7 @@
import { onMount } from 'svelte';
import { humanFileSize } from '$lib/utilities';
import socketService from '$lib/services/socket-service';
let isConnected = socketService.isConnected;
let settings = socketService.settings;
let systemInfo = socketService.systemInfo;
import { isConnected, systemInfo } from '$lib/stores';
onMount(() => {
if ($isConnected) {
app/src/components/settings/Log.svelte
+2 -4
View File
@@ -1,10 +1,8 @@
<script lang="ts">
import socketService from '$lib/services/socket-service';
import { isConnected, logs } from '$lib/stores';
import { onMount } from 'svelte';
let isConnected = socketService.isConnected;
let log = socketService.log;
onMount(() => {
if ($isConnected) {
const message = JSON.stringify({ type: 'system/logs' });
@@ -14,7 +12,7 @@
</script>
<div class="w-full h-full">
{#each $log as entry}
{#each $logs as entry}
<div>{entry}</div>
{/each}
</div>
app/src/lib/models.ts
+11 -2
View File
@@ -1,11 +1,20 @@
export type vector = { x: number; y: number };
export interface ControllerInput {
left: vector;
right: vector;
height: number;
speed: number;
}
export type angles = number[] | Int16Array;
type AnglesData = {
export type AnglesData = {
type: 'angles';
data: angles;
};
type LogData = {
export type LogData = {
type: 'log';
data: string;
};
app/src/lib/services/socket-service.ts
+3 -1
View File
@@ -39,7 +39,9 @@ class SocketService {
}
public addPublisher(store: Writable<WebsocketOutData>, type?: string) {
store.subscribe((data) => this.send(type ? JSON.stringify({ type, data }) : data));
const publish = (data: WebsocketOutData) =>
this.send(type ? JSON.stringify({ type, data }) : data);
store.subscribe(publish);
}
private handleConnected(): void {
app/src/lib/stores/index.ts
+1
View File
@@ -1,2 +1,3 @@
export * from './socket-store';
export * from './logging-store';
export * from './model-store';
app/src/lib/store.ts → app/src/lib/stores/model-store.ts
+15 -8
View File
@@ -1,17 +1,24 @@
import { writable } from 'svelte/store';
import type { ControllerInput } from '$lib/models';
import { persistentStore } from '$lib/utilities';
import { writable, type Writable } from 'svelte/store';
export const emulateModel = writable(true);
export const input = writable({
export const jointNames = persistentStore('joint_names', []);
export const model = writable();
export const modes = ['idle', 'stand', 'walk'] as const;
export type Modes = (typeof modes)[number];
export const mode: Writable<Modes> = writable('idle');
export const outControllerData = writable(new Int8Array([0, 0, 0, 0, 0, 70, 0]));
export const input: Writable<ControllerInput> = writable({
left: { x: 0, y: 0 },
right: { x: 0, y: 0 },
height: 70,
speed: 0
});
export const outControllerData = writable(new Int8Array([0, 0, 0, 0, 0, 70, 0]));
export const jointNames = persistentStore('joint_names', []);
export const model = writable();
app/src/lib/utilities/math-utilities.ts
+4 -4
View File
@@ -5,7 +5,7 @@ export const toUint8 = (number: number, min: number, max: number) => {
};
export const toInt8 = (number: number, min: number, max: number) => {
number = Math.max(min, Math.min(max, number));
let scaled = ((number - min) / (max - min)) * 255 - 128;
return Math.max(-128, Math.min(127, Math.round(scaled))) | 0;
};
number = Math.max(min, Math.min(max, number));
let scaled = ((number - min) / (max - min)) * 255 - 128;
return Math.max(-128, Math.min(127, Math.round(scaled))) | 0;
};
app/src/lib/utilities/string-utilities.ts
+4
View File
@@ -3,3 +3,7 @@ export const humanFileSize = (size: number): string => {
var i = size == 0 ? 0 : Math.floor(Math.log(size) / Math.log(1024));
return Number((size / Math.pow(1024, i)).toFixed(2)) * 1 + units[i];
};
export const capitalize = (str: string): string => {
return str.charAt(0).toUpperCase() + str.slice(1).toLowerCase();
};
app/src/main.ts
+1 -1
View File
@@ -3,7 +3,7 @@ import './index.css';
import App from './App.svelte';
const app = new App({
target: document.getElementById('app')
target: document.getElementById('app') as HTMLElement
});
export default app;
app/src/routes/Controller.svelte
+1 -1
View File
@@ -2,7 +2,7 @@
import Stream from '$components/Views/Stream.svelte';
import Model from '$components/Views/Model.svelte';
import Controls from '$components/Controls.svelte';
import { emulateModel } from '$lib/store';
import { emulateModel } from '$lib/stores';
</script>
<div class="flex justify-center items-center w-full h-full">
app/src/routes/Settings.svelte
+1 -4
View File
@@ -5,13 +5,10 @@
import Configuration from '../components/settings/Configuration.svelte';
import {
Icon,
Wifi,
CommandLine,
InformationCircle,
BookOpen,
AdjustmentsVertical,
Cog6Tooth,
Newspaper
Cog6Tooth
} from 'svelte-hero-icons';
import Calibration from '../components/settings/Calibration.svelte';
app/test/specs/number-convert.spec.ts
-2
View File
@@ -42,5 +42,3 @@ describe('toInt8', () => {
expect(toInt8(2, -1, 1)).toBe(127);
});
});
mock/server.js
+122 -36
View File
@@ -19,22 +19,29 @@ const randomFloatFromInterval = (min, max) =>
const radToDeg = (val) => val * (180 / Math.PI);
const degToRad = (val) => val * (Math.PI / 180);
const lerp = (start, end, amt) => {
return (1 - amt) * start + amt * end;
};
function createNewClientState() {
return {
model: JSON.parse(JSON.stringify(model)),
settings: JSON.parse(JSON.stringify(settings)),
logs: JSON.parse(JSON.stringify(logs)),
subscriptions: {},
mode: "idle",
controller: {
stop: 0,
lx: 0,
ly: 0,
rx: 0,
ry: 0,
h: 70,
s: 0,
},
};
}
function subscribeClientToCategory(ws, category) {
if (!subscriptions[category]) {
subscriptions[category] = new Set();
}
subscriptions[category].add(ws);
}
const unsubscribeClientFromCategory = (ws, category) => {
if (!subscriptions[category]) return;
subscriptions[category].delete(ws);
@@ -52,11 +59,11 @@ const sendUpdateToSubscribers = (category, data) => {
}
};
if (!Array.prototype.last){
Array.prototype.last = function(){
return this[this.length - 1];
};
};
if (!Array.prototype.last) {
Array.prototype.last = function () {
return this[this.length - 1];
};
}
const model = {
battery: {
@@ -90,6 +97,8 @@ const model = {
},
running: true,
mode: "stand",
rotation: [0, 0, 0],
position: [0, 0, 0],
};
const settings = {
@@ -217,28 +226,111 @@ const unpackMessageBuffer = (data) => {
};
};
const updateStanding = (ws, controller) => {
if (!ws.clientState.model.running) return;
const data = unpackMessageBuffer(controller);
ws.send(
const rest = {
rotation: [0, 0, 0],
position: [0, 10, 0],
};
const idle = (client) => {
for (let i = 0; i < 3; i++) {
client.clientState.model.position[i] = lerp(
client.clientState.model.position[i],
rest.position[i],
0.01
);
client.clientState.model.rotation[i] = lerp(
client.clientState.model.rotation[i],
rest.rotation[i],
0.01
);
}
client.send(
JSON.stringify({
type: "angles",
data: updateBodyState(ws.clientState.model, data.angles, data.position),
data: updateBodyState(
client.clientState.model,
client.clientState.model.rotation,
client.clientState.model.position
),
})
);
};
const stand = (client) => {
if (!client.clientState.model.running) return;
const data = unpackMessageBuffer(client.clientState.controller);
client.send(
JSON.stringify({
type: "angles",
data: updateBodyState(
client.clientState.model,
data.angles,
data.position
),
})
);
};
// https://www.hindawi.com/journals/cin/2016/9853070/
const step = (model, controller, tick) => {
const y1 = -100 * Math.sin(-0.05 * tick) - 150;
const y2 = -100 * Math.sin(-0.05 * tick + Math.PI) - 150;
const x1 = Math.abs((tick % 120) - 60) - 60;
const Lp = [
// -50 is minimum
[100, y1, 100, 1],
[100, y2, -100, 1],
[-100, y2, 100, 1],
[-100, y1, -100, 1],
];
model.servos.angles = kinematic
.calcIK(
Lp,
model.rotation.map((x) => degToRad(x)),
model.position
)
.flat()
.map((x, i) => radToDeg(x * model.servos.dir[i]));
return model.servos.angles;
};
const walk = (client) => {
const angles = step(
client.clientState.model,
client.clientState.controller,
client.tick
);
client.send(JSON.stringify({ type: "angles", data: angles }));
};
const start_dynamics = (client) => {
client.tick = 0;
client.clientState.mode = "idle";
client.clientState.next_mode = "walk";
const modes = { idle, stand, walk };
client.id = setInterval(() => {
client.tick += 1;
if (client.clientState.mode !== client.clientState.next_mode) {
// Transition
client.clientState.mode = client.clientState.next_mode;
} else {
modes[client.clientState.mode](client);
}
}, 10);
};
const handelController = (ws, buffer) => {
const controllerData = bufferToController(new Int8Array(buffer));
ws.clientState.controller = controllerData;
if (controllerData.stop) {
ws.clientState.model.running = false;
ws.clientState.logs.push("[2024-02-05 19:10:00] [Warning] STOPPING SERVOS");
ws.send(JSON.stringify({ type: "log", data: ws.clientState.logs.last() }));
return;
}
if (ws.clientState.model.mode === "stand") {
updateStanding(ws, controllerData);
}
};
const handleBufferMessage = (ws, buffer) => {
@@ -247,19 +339,11 @@ const handleBufferMessage = (ws, buffer) => {
}
};
const handleJsonMessage = (ws, message) => {
let data = message;
try {
data = JSON.parse(message);
} catch (error) {
return;
}
const handleJsonMessage = (ws, data) => {
switch (data.type) {
case "subscribe":
subscribeClientToCategory(ws, data.category);
break;
case "unsubscribe":
unsubscribeClientFromCategory(ws, data.category);
case "mode":
ws.clientState.next_mode = data.data;
// ws.send({ type: "battery", data: JSON.stringify(updateBattery()) });
break;
case "sensor/battery":
ws.send({ type: "battery", data: JSON.stringify(updateBattery()) });
@@ -353,15 +437,17 @@ const handleJsonMessage = (ws, message) => {
wss.on("connection", (ws) => {
const clientState = createNewClientState();
ws.clientState = clientState;
start_dynamics(ws);
ws.on("error", console.error);
ws.on("message", (message) => {
if (typeof message === "object") {
let data = message;
try {
data = JSON.parse(message);
handleJsonMessage(ws, data);
} catch (error) {
handleBufferMessage(ws, message);
return;
}
handleJsonMessage(ws, message);
});
ws.on("close", () => {