diff --git a/app/src/lib/components/Visualization.svelte b/app/src/lib/components/Visualization.svelte index 6e3fa2d..7bba1b8 100644 --- a/app/src/lib/components/Visualization.svelte +++ b/app/src/lib/components/Visualization.svelte @@ -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!)) diff --git a/app/src/lib/platform_shared/websocket_message.ts b/app/src/lib/platform_shared/websocket_message.ts index c6f2e6f..4978a36 100644 --- a/app/src/lib/platform_shared/websocket_message.ts +++ b/app/src/lib/platform_shared/websocket_message.ts @@ -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 = { }, }; +function createBaseKinematicData(): KinematicData { + return { omega: 0, phi: 0, psi: 0, xm: 0, ym: 0, zm: 0 }; +} + +export const KinematicData: MessageFns = { + 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>>(base?: I): KinematicData { + return KinematicData.fromPartial(base ?? ({} as any)); + }, + fromPartial, 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 = { 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 = { 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 = { 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 = { 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 = { 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, diff --git a/app/src/lib/stores/model-store.ts b/app/src/lib/stores/model-store.ts index f31f592..bdf8ff7 100644 --- a/app/src/lib/stores/model-store.ts +++ b/app/src/lib/stores/model-store.ts @@ -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 = 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 = writable( HumanInputData.create( {left: {x:0,y:0}, right: {x:0,y:0}, height:0, s1:0, speed:0} ) ) diff --git a/app/src/lib/stores/socket-store.ts b/app/src/lib/stores/socket-store.ts index c84a903..2c837ee 100644 --- a/app/src/lib/stores/socket-store.ts +++ b/app/src/lib/stores/socket-store.ts @@ -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 = writable([ - 0, 45, -90, 0, 45, -90, 0, 45, -90, 0, 45, -90 -]) -export const servoAngles: Writable = writable([ - 0, 45, -90, 0, 45, -90, 0, 45, -90, 0, 45, -90 -]) +export const servoAnglesOut: Writable = writable(AnglesData.create({angles: [0, 45, -90, 0, 45, -90, 0, 45, -90, 0, 45, -90]})) +export const servoAngles: Writable = 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: Writable logs: Writable mpu: Writable distances: Writable diff --git a/app/src/routes/+layout.svelte b/app/src/routes/+layout.svelte index 2b0a166..9638bb7 100644 --- a/app/src/routes/+layout.svelte +++ b/app/src/routes/+layout.svelte @@ -21,7 +21,7 @@ useFeatureFlags, walkGait } from '$lib/stores' - import { AnalyticsData, AnglesData, DownloadOTAData, HumanInputData, ModeData, RSSIData, SonarData } from '$lib/platform_shared/websocket_message' + import { AnalyticsData, AnglesData, DownloadOTAData, HumanInputData, KinematicData, ModeData, RSSIData, SonarData, WalkGaitData } from '$lib/platform_shared/websocket_message' import { Throttler } from '$lib/utilities' interface Props { @@ -41,9 +41,9 @@ outControllerData.subscribe(data => socket.sendEvent(HumanInputData, data)) mode.subscribe(data => socket.sendEvent(ModeData, data)) - walkGait.subscribe(data => socket.sendEvent(GaitData, data)) + walkGait.subscribe(data => socket.sendEvent(WalkGaitData, data)) servoAnglesOut.subscribe(data => socket.sendEvent(AnglesData, data)) - kinematicData.subscribe(data => socket.sendEvent(PositionData, data)) + kinematicData.subscribe(data => socket.sendEvent(KinematicData, data)) }) onDestroy(() => { diff --git a/platform_shared/websocket_message.proto b/platform_shared/websocket_message.proto index ed55beb..501824b 100644 --- a/platform_shared/websocket_message.proto +++ b/platform_shared/websocket_message.proto @@ -13,7 +13,6 @@ message IMUData { float temp = 4; } -DEACTIVATED = 0; enum ModesEnum { DEACTIVATED = 0; IDLE = 1; @@ -83,6 +82,14 @@ enum WalkGaits { message WalkGaitData { WalkGaits gait = 1; } +message KinematicData { + float omega = 1; + float phi = 2; + float psi = 3; + float xm = 4; + float ym = 5; + float zm = 6; +} message SubscribeNotification { int32 tag = 1; } message UnsubscribeNotification {int32 tag = 1; } @@ -107,6 +114,7 @@ message WebsocketMessage { AnglesData angles = 170; I2CScanData i2c_scan = 180; PeripheralSettingsData peripheral_settings = 190; + KinematicData kinematic_data = 200; WifiSettingsData wifi_settings = 240; HumanInputData human_input_data = 250; RSSIData rssi = 260;