Updated servo angles and kinematic data

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([
+        kinematicData.set(KinematicData.create({
-            settings.omega,
+            omega: settings.omega,
-            settings.phi,
+            phi: settings.phi,
-            settings.psi,
+            psi: settings.psi,
-            settings.xm,
+            xm: settings.xm,
-            settings.ym,
+            ym: settings.ym,
-            settings.zm
+            zm: settings.zm
-        ])
+        }))
     }
 
     const setSceneBackground = (c: string | null) => (sceneManager.scene.background = new Color(c!))

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<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],
+        "path": [4, 25],
-        "span": [94, 0, 112, 1],
+        "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

@@ -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

@@ -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([
+export const servoAnglesOut: Writable<AnglesData> = writable(AnglesData.create({angles: [0, 45, -90, 0, 45, -90, 0, 45, -90, 0, 45, -90]}))
-    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 servoAngles: Writable<number[]> = writable([
-    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>

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(() => {

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;