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

Updated servo angles and kinematic data

Browse Source
This commit is contained in:
Niklas Jensen
2026-01-02 12:12:40 +01:00
committed by nikguin04
parent f25aba5f29
commit cdaa60d0e1
6 changed files with 297 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/src/lib/components/Visualization.svelte
+10 -11
View File
@@ -10,8 +10,6 @@
Color
} from 'three'
import {
ModesEnum,
kinematicData,
mode,
model,
input,
@@ -21,7 +19,7 @@
jointNames,
currentKinematic,
walkGait,
walkGaitToMode
kinematicData,
} from '$lib/stores'
import { populateModelCache, getToeWorldPositions } from '$lib/utilities'
import SceneBuilder from '$lib/sceneBuilder'
@@ -32,6 +30,7 @@
import { radToDeg } from 'three/src/math/MathUtils.js'
import type { URDFRobot } from 'urdf-loader'
import { get } from 'svelte/store'
import { KinematicData } from '$lib/platform_shared/websocket_message'
interface Props {
defaultColor?: string | null
@@ -155,14 +154,14 @@
}
const updateKinematicPosition = () => {
kinematicData.set([
settings.omega,
settings.phi,
settings.psi,
settings.xm,
settings.ym,
settings.zm
])
kinematicData.set(KinematicData.create({
omega: settings.omega,
phi: settings.phi,
psi: settings.psi,
xm: settings.xm,
ym: settings.ym,
zm: settings.zm
}))
}
const setSceneBackground = (c: string | null) => (sceneManager.scene.background = new Color(c!))
app/src/lib/platform_shared/websocket_message.ts
+268 -2
View File
@@ -237,6 +237,15 @@ export interface WalkGaitData {
gait: WalkGaits;
}
export interface KinematicData {
omega: number;
phi: number;
psi: number;
xm: number;
ym: number;
zm: number;
}
export interface SubscribeNotification {
tag: number;
}
@@ -268,6 +277,7 @@ export interface WebsocketMessage {
angles?: AnglesData | undefined;
i2cScan?: I2CScanData | undefined;
peripheralSettings?: PeripheralSettingsData | undefined;
kinematicData?: KinematicData | undefined;
wifiSettings?: WifiSettingsData | undefined;
humanInputData?: HumanInputData | undefined;
rssi?: RSSIData | undefined;
@@ -2421,6 +2431,146 @@ export const WalkGaitData: MessageFns<WalkGaitData> = {
},
};
function createBaseKinematicData(): KinematicData {
return { omega: 0, phi: 0, psi: 0, xm: 0, ym: 0, zm: 0 };
}
export const KinematicData: MessageFns<KinematicData> = {
encode(message: KinematicData, writer: BinaryWriter = new BinaryWriter()): BinaryWriter {
if (message.omega !== 0) {
writer.uint32(13).float(message.omega);
}
if (message.phi !== 0) {
writer.uint32(21).float(message.phi);
}
if (message.psi !== 0) {
writer.uint32(29).float(message.psi);
}
if (message.xm !== 0) {
writer.uint32(37).float(message.xm);
}
if (message.ym !== 0) {
writer.uint32(45).float(message.ym);
}
if (message.zm !== 0) {
writer.uint32(53).float(message.zm);
}
return writer;
},
decode(input: BinaryReader | Uint8Array, length?: number): KinematicData {
const reader = input instanceof BinaryReader ? input : new BinaryReader(input);
const end = length === undefined ? reader.len : reader.pos + length;
const message = createBaseKinematicData();
while (reader.pos < end) {
const tag = reader.uint32();
switch (tag >>> 3) {
case 1: {
if (tag !== 13) {
break;
}
message.omega = reader.float();
continue;
}
case 2: {
if (tag !== 21) {
break;
}
message.phi = reader.float();
continue;
}
case 3: {
if (tag !== 29) {
break;
}
message.psi = reader.float();
continue;
}
case 4: {
if (tag !== 37) {
break;
}
message.xm = reader.float();
continue;
}
case 5: {
if (tag !== 45) {
break;
}
message.ym = reader.float();
continue;
}
case 6: {
if (tag !== 53) {
break;
}
message.zm = reader.float();
continue;
}
}
if ((tag & 7) === 4 || tag === 0) {
break;
}
reader.skip(tag & 7);
}
return message;
},
fromJSON(object: any): KinematicData {
return {
omega: isSet(object.omega) ? globalThis.Number(object.omega) : 0,
phi: isSet(object.phi) ? globalThis.Number(object.phi) : 0,
psi: isSet(object.psi) ? globalThis.Number(object.psi) : 0,
xm: isSet(object.xm) ? globalThis.Number(object.xm) : 0,
ym: isSet(object.ym) ? globalThis.Number(object.ym) : 0,
zm: isSet(object.zm) ? globalThis.Number(object.zm) : 0,
};
},
toJSON(message: KinematicData): unknown {
const obj: any = {};
if (message.omega !== 0) {
obj.omega = message.omega;
}
if (message.phi !== 0) {
obj.phi = message.phi;
}
if (message.psi !== 0) {
obj.psi = message.psi;
}
if (message.xm !== 0) {
obj.xm = message.xm;
}
if (message.ym !== 0) {
obj.ym = message.ym;
}
if (message.zm !== 0) {
obj.zm = message.zm;
}
return obj;
},
create<I extends Exact<DeepPartial<KinematicData>, I>>(base?: I): KinematicData {
return KinematicData.fromPartial(base ?? ({} as any));
},
fromPartial<I extends Exact<DeepPartial<KinematicData>, I>>(object: I): KinematicData {
const message = createBaseKinematicData();
message.omega = object.omega ?? 0;
message.phi = object.phi ?? 0;
message.psi = object.psi ?? 0;
message.xm = object.xm ?? 0;
message.ym = object.ym ?? 0;
message.zm = object.zm ?? 0;
return message;
},
};
function createBaseSubscribeNotification(): SubscribeNotification {
return { tag: 0 };
}
@@ -2637,6 +2787,7 @@ function createBaseWebsocketMessage(): WebsocketMessage {
angles: undefined,
i2cScan: undefined,
peripheralSettings: undefined,
kinematicData: undefined,
wifiSettings: undefined,
humanInputData: undefined,
rssi: undefined,
@@ -2681,6 +2832,9 @@ export const WebsocketMessage: MessageFns<WebsocketMessage> = {
if (message.peripheralSettings !== undefined) {
PeripheralSettingsData.encode(message.peripheralSettings, writer.uint32(1522).fork()).join();
}
if (message.kinematicData !== undefined) {
KinematicData.encode(message.kinematicData, writer.uint32(1602).fork()).join();
}
if (message.wifiSettings !== undefined) {
WifiSettingsData.encode(message.wifiSettings, writer.uint32(1922).fork()).join();
}
@@ -2796,6 +2950,14 @@ export const WebsocketMessage: MessageFns<WebsocketMessage> = {
message.peripheralSettings = PeripheralSettingsData.decode(reader, reader.uint32());
continue;
}
case 200: {
if (tag !== 1602) {
break;
}
message.kinematicData = KinematicData.decode(reader, reader.uint32());
continue;
}
case 240: {
if (tag !== 1922) {
break;
@@ -2845,6 +3007,7 @@ export const WebsocketMessage: MessageFns<WebsocketMessage> = {
peripheralSettings: isSet(object.peripheralSettings)
? PeripheralSettingsData.fromJSON(object.peripheralSettings)
: undefined,
kinematicData: isSet(object.kinematicData) ? KinematicData.fromJSON(object.kinematicData) : undefined,
wifiSettings: isSet(object.wifiSettings) ? WifiSettingsData.fromJSON(object.wifiSettings) : undefined,
humanInputData: isSet(object.humanInputData) ? HumanInputData.fromJSON(object.humanInputData) : undefined,
rssi: isSet(object.rssi) ? RSSIData.fromJSON(object.rssi) : undefined,
@@ -2889,6 +3052,9 @@ export const WebsocketMessage: MessageFns<WebsocketMessage> = {
if (message.peripheralSettings !== undefined) {
obj.peripheralSettings = PeripheralSettingsData.toJSON(message.peripheralSettings);
}
if (message.kinematicData !== undefined) {
obj.kinematicData = KinematicData.toJSON(message.kinematicData);
}
if (message.wifiSettings !== undefined) {
obj.wifiSettings = WifiSettingsData.toJSON(message.wifiSettings);
}
@@ -2938,6 +3104,9 @@ export const WebsocketMessage: MessageFns<WebsocketMessage> = {
message.peripheralSettings = (object.peripheralSettings !== undefined && object.peripheralSettings !== null)
? PeripheralSettingsData.fromPartial(object.peripheralSettings)
: undefined;
message.kinematicData = (object.kinematicData !== undefined && object.kinematicData !== null)
? KinematicData.fromPartial(object.kinematicData)
: undefined;
message.wifiSettings = (object.wifiSettings !== undefined && object.wifiSettings !== null)
? WifiSettingsData.fromPartial(object.wifiSettings)
: undefined;
@@ -4136,6 +4305,90 @@ export const protoMetadata: ProtoMetadata = {
"reservedRange": [],
"reservedName": [],
"visibility": 0,
}, {
"name": "KinematicData",
"field": [{
"name": "omega",
"number": 1,
"label": 1,
"type": 2,
"typeName": "",
"extendee": "",
"defaultValue": "",
"oneofIndex": 0,
"jsonName": "omega",
"options": undefined,
"proto3Optional": false,
}, {
"name": "phi",
"number": 2,
"label": 1,
"type": 2,
"typeName": "",
"extendee": "",
"defaultValue": "",
"oneofIndex": 0,
"jsonName": "phi",
"options": undefined,
"proto3Optional": false,
}, {
"name": "psi",
"number": 3,
"label": 1,
"type": 2,
"typeName": "",
"extendee": "",
"defaultValue": "",
"oneofIndex": 0,
"jsonName": "psi",
"options": undefined,
"proto3Optional": false,
}, {
"name": "xm",
"number": 4,
"label": 1,
"type": 2,
"typeName": "",
"extendee": "",
"defaultValue": "",
"oneofIndex": 0,
"jsonName": "xm",
"options": undefined,
"proto3Optional": false,
}, {
"name": "ym",
"number": 5,
"label": 1,
"type": 2,
"typeName": "",
"extendee": "",
"defaultValue": "",
"oneofIndex": 0,
"jsonName": "ym",
"options": undefined,
"proto3Optional": false,
}, {
"name": "zm",
"number": 6,
"label": 1,
"type": 2,
"typeName": "",
"extendee": "",
"defaultValue": "",
"oneofIndex": 0,
"jsonName": "zm",
"options": undefined,
"proto3Optional": false,
}],
"extension": [],
"nestedType": [],
"enumType": [],
"extensionRange": [],
"oneofDecl": [],
"options": undefined,
"reservedRange": [],
"reservedName": [],
"visibility": 0,
}, {
"name": "SubscribeNotification",
"field": [{
@@ -4354,6 +4607,18 @@ export const protoMetadata: ProtoMetadata = {
"jsonName": "peripheralSettings",
"options": undefined,
"proto3Optional": false,
}, {
"name": "kinematic_data",
"number": 200,
"label": 1,
"type": 11,
"typeName": ".KinematicData",
"extendee": "",
"defaultValue": "",
"oneofIndex": 0,
"jsonName": "kinematicData",
"options": undefined,
"proto3Optional": false,
}, {
"name": "wifi_settings",
"number": 240,
@@ -4438,8 +4703,8 @@ export const protoMetadata: ProtoMetadata = {
"trailingComments": "",
"leadingDetachedComments": [],
}, {
"path": [4, 24],
"span": [94, 0, 112, 1],
"path": [4, 25],
"span": [102, 0, 121, 1],
"leadingComments": " WebSocket message wrapper\n Only ONE field will be set at a time (oneof ensures this)\n",
"trailingComments": "",
"leadingDetachedComments": [],
@@ -4471,6 +4736,7 @@ export const protoMetadata: ProtoMetadata = {
".HumanInputData": HumanInputData,
".SystemInformation": SystemInformation,
".WalkGaitData": WalkGaitData,
".KinematicData": KinematicData,
".SubscribeNotification": SubscribeNotification,
".UnsubscribeNotification": UnsubscribeNotification,
".PingMsg": PingMsg,
app/src/lib/stores/model-store.ts
+2 -2
View File
@@ -1,4 +1,4 @@
import { HumanInputData, ModeData, ModesEnum, WalkGaitData, WalkGaits } from '$lib/platform_shared/websocket_message'
import { HumanInputData, KinematicData, ModeData, ModesEnum, WalkGaitData, WalkGaits } from '$lib/platform_shared/websocket_message'
import { persistentStore } from '$lib/utilities/svelte-utilities'
import { writable, type Writable } from 'svelte/store'
@@ -14,7 +14,7 @@ export const walkGait: Writable<WalkGaitData> = writable( WalkGaitData.create({g
export const outControllerData = writable( HumanInputData.create( {left: {x:0,y:0}, right: {x:0,y:0}, height:0, s1:0, speed:0} ) )
export const kinematicData = writable([0, 0, 0, 0, 1, 0])
export const kinematicData = writable(KinematicData.create())
export const input: Writable<HumanInputData> = writable( HumanInputData.create( {left: {x:0,y:0}, right: {x:0,y:0}, height:0, s1:0, speed:0} ) )
app/src/lib/stores/socket-store.ts
+5 -8
View File
@@ -1,19 +1,16 @@
import { AnglesData } from '$lib/platform_shared/websocket_message'
import { writable, type Writable } from 'svelte/store'
import { type angles } from '$lib/types/models'
export const servoAnglesOut: Writable<number[]> = writable([
0, 45, -90, 0, 45, -90, 0, 45, -90, 0, 45, -90
])
export const servoAngles: Writable<number[]> = writable([
0, 45, -90, 0, 45, -90, 0, 45, -90, 0, 45, -90
])
export const servoAnglesOut: Writable<AnglesData> = writable(AnglesData.create({angles: [0, 45, -90, 0, 45, -90, 0, 45, -90, 0, 45, -90]}))
export const servoAngles: Writable<AnglesData> = writable(AnglesData.create({angles: [0, 45, -90, 0, 45, -90, 0, 45, -90, 0, 45, -90]}))
export const logs = writable([] as string[])
export const mpu = writable({ heading: 0 })
export const sonar = writable([0, 0])
export const distances = writable({})
export interface socketDataCollection {
angles: Writable<angles>
angles: Writable<AnglesData>
logs: Writable<string[]>
mpu: Writable<unknown>
distances: Writable<unknown>