Mocked walking gait experiment

.gitignore

@@ -0,0 +1,2 @@
+*.pyc
+spot_env

app/src/components/Controls.svelte

@@ -12,7 +12,7 @@
 	let right: nipplejs.JoystickManager;
 
 	let throttle_timing = 40;
-	let data = new Int8Array(7);
+	let data = new Int8Array($outControllerData.length);
 
 	onMount(() => {
 		left = nipplejs.create({
@@ -46,13 +46,14 @@
 	};
 
 	const updateData = () => {
-		data[0] = 0;
-		data[1] = toInt8($input.left.x, -1, 1);
-		data[2] = toInt8($input.left.y, -1, 1);
-		data[3] = toInt8($input.right.x, -1, 1);
-		data[4] = toInt8($input.right.y, -1, 1);
-		data[5] = toInt8($input.height, 0, 100);
-		data[6] = toInt8($input.speed, 0, 100);
+		data[0] = 1;
+		data[1] = 0;
+		data[2] = toInt8($input.left.x, -1, 1);
+		data[3] = toInt8($input.left.y, -1, 1);
+		data[4] = toInt8($input.right.x, -1, 1);
+		data[5] = toInt8($input.right.y, -1, 1);
+		data[6] = toInt8($input.height, 0, 100);
+		data[7] = toInt8($input.speed, 0, 100);
 
 		outControllerData.set(data);
 	};

app/src/components/Views/Model.svelte

@@ -27,7 +27,8 @@
 
     let settings = {
         'Trace feet':true,
-        'Trace points': 30
+        'Trace points': 30,
+        'Fix camera on robot': true
     }
 
 	onMount(async () => {
@@ -48,6 +49,7 @@
         const visibility = panel.addFolder('Visualization');
         visibility.add(settings, 'Trace feet')
         visibility.add(settings, 'Trace points', 1, 1000, 1)
+        visibility.add(settings, 'Fix camera on robot')
     }
 
 	const cacheModelFiles = async () => {
@@ -142,6 +144,10 @@
 
         renderTraceLines(toes)
 
+        if (settings['Fix camera on robot']) {
+            sceneManager.controls.target = robot.position.clone()
+        }
+
 		robot.position.y = robot.position.y - Math.min(...toes.map(toe => toe.y));
 		robot.rotation.z = lerp(robot.rotation.z, degToRad($mpu.heading + 90), 0.1);
 		modelTargetAngles = $servoAngles;

app/src/lib/sceneBuilder.ts

@@ -197,6 +197,7 @@ export default class SceneBuilder {
 
 	public startRenderLoop = () => {
 		this.renderer.setAnimationLoop(() => {
+            this.controls.update();
 			this.renderer.render(this.scene, this.camera);
 			this.handleRobotShadow();
 			if (this.callback) this.callback();

app/src/lib/services/socket-service.ts

@@ -17,10 +17,12 @@ type WebsocketOutData = string | ArrayBufferLike | Blob | ArrayBufferView;
 
 class SocketService {
 	private socket!: WebSocket;
+    private url?:string
 
 	constructor() {}
 
 	public connect(url: string): void {
+        this.url = url
 		this.socket = new WebSocket(url);
 		this.socket.binaryType = 'arraybuffer';
 		this.socket.onopen = () => this.handleConnected();
@@ -50,6 +52,7 @@ class SocketService {
 
 	private handleDisconnected(): void {
 		isConnected.set(false);
+        setTimeout(() => this.connect(this.url as string), 500)
 	}
 
 	private getJsonFromMessage(msg: string): Result<WebSocketJsonMsg, string> {

app/src/lib/stores/model-store.ts

@@ -14,7 +14,7 @@ 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 outControllerData = writable(new Int8Array([0, 0, 0, 0, 0, 0, 70, 0]));
 
 export const input: Writable<ControllerInput> = writable({
 	left: { x: 0, y: 0 },

mock/.gitignore

@@ -1 +1,3 @@
-/node_modules
+/node_modules
+__pycache__/
+*.pyc

mock/MotionController.py

@@ -0,0 +1,53 @@
+import copy
+
+
+class GaitState:
+    def __init__(self) -> None:
+        self.step_length = 0.1
+        self.yaw_rate = 0
+        self.lateral_fraction = 0
+        self.step_velocity = 0.001
+        self.swing_period = 0.2
+        self.clearance_height = 0.045
+        self.penetration_depth = 0.003
+        self.contacts = [False] * 4
+
+        self.target_step_length = 0
+        self.target_yaw_rate = 0
+        self.target_lateral_fraction = 0
+
+    def update_gait_state(self, dt):
+        self.step_length = self.step_length * (1 - dt) + self.target_step_length * dt
+        self.lateral_fraction = (
+            self.lateral_fraction * (1 - dt) + self.target_lateral_fraction * dt
+        )
+        self.yaw_rate = self.yaw_rate * (1 - dt) + self.target_yaw_rate * dt
+
+
+class MotionController:
+    def __init__(
+        self,
+        # env: spotBezierEnv,
+        # gui: GUI,
+        # bodyState: BodyState,
+        # gaitState: GaitState,
+        spot_model,
+        gait,
+    ) -> None:
+        self.gait = gait
+        self.gait_state = GaitState()
+        self.spot_model = spot_model
+
+        self.dt = 0.01
+
+    def update_gait_state(self, command):
+        self.gait_state.step_length = abs(command["lx"]) / 255
+
+    def run(self, model, command):
+        self.update_gait_state(command)
+        self.gait_state.contacts = [False] * 4
+        self.body_state.worldFeetPositions = copy.deepcopy(self.spot.WorldToFoot)
+
+        model["transformation"]["world_feet_position"] = self.gait.generate_trajectory(
+            model, self.gait_state, self.dt
+        )

mock/model.py

@@ -0,0 +1,34 @@
+import random
+
+
+model = lambda: {
+    "gait": {
+        "step_length": 0,
+        "yaw_rate": 0,
+        "lateral_fraction": 0,
+        "step_velocity": 0,
+        "swing_period": 0,
+        "clearance_height": 0,
+        "penetration_depth": 0,
+        "contacts": 0,
+    },
+    "transformation": {
+        "world_position": [0, 0, 0],
+        "position": [0, 0, 0],
+        "rotation": [0, 0, 0],
+        "world_feet_positions": {},
+    },
+    "sensors": {
+        "mpu": {
+            "x": 0,
+            "y": 0,
+            "z": 0,
+        },
+        "battery": {
+            "voltage": round(random.uniform(7.6, 8.2), 2),
+            "ampere": round(random.uniform(0.2, 3), 2),
+        },
+    },
+    "logs": ["[2023-02-05 10:00:00] Booting up"],
+    "settings": {"useMetric": True},
+}

mock/server.js

@@ -209,13 +209,14 @@ const updateAngles = (angles) => {
 
 const bufferToController = (buffer) => {
   return {
-    stop: buffer[0],
-    lx: buffer[1],
-    ly: buffer[2],
-    rx: buffer[3],
-    ry: buffer[4],
-    h: buffer[5],
-    s: buffer[6],
+    command: buffer[0],
+    stop: buffer[1],
+    lx: buffer[2],
+    ly: buffer[3],
+    rx: buffer[4],
+    ry: buffer[5],
+    h: buffer[6],
+    s: buffer[7],
   };
 };
 
@@ -276,15 +277,31 @@ const stand = (client) => {
 // 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 arc_height = controller.h;
+  const speed = (controller.s + 128) / 255 / 4;
+
+  const T_stride_s = 500 / 1000;
+  const overlay = 10 / 100;
+
+  const T_stance_s = T_stride_s * (0.5 + overlay);
+  const T_swing_s = T_stride_s * (0.5 - overlay);
+
+  const x = Math.abs((tick % 200) - 100);
+
+  const y1 = Math.min(
+    Math.max(-arc_height * Math.sin(-speed * tick) - 150, -200),
+    -100
+  );
+  const y2 = Math.min(
+    Math.max(-arc_height * Math.sin(-speed * tick + Math.PI) - 150, -200),
+    -100
+  );
   const Lp = [
     // -50  is minimum
     [100, y1, 100, 1],
     [100, y2, -100, 1],
-    [-100, y2, 100, 1],
-    [-100, y1, -100, 1],
+    [-65, y2, 100, 1],
+    [-65, y1, -100, 1],
   ];
 
   model.servos.angles = kinematic
@@ -336,7 +353,7 @@ const handelController = (ws, buffer) => {
 };
 
 const handleBufferMessage = (ws, buffer) => {
-  if (buffer.length === 6) {
+  if (buffer.length === 8) {
     handelController(ws, buffer);
   }
 };

mock/server.py

@@ -0,0 +1,100 @@
+import asyncio
+from enum import Enum
+import json
+import sys
+import websockets
+from model import model
+from MotionController import GaitState
+from simulator.GaitGenerator.Bezier import BezierGait
+from simulator.GymEnvs.spot_bezier_env import spotBezierEnv
+from simulator.Kinematics.SpotKinematics import SpotModel
+from simulator.util.gui import GUI
+from simulator.simulator import BodyState, Simulator
+import struct
+
+sys.path.append("./simulator/GymEnvs")
+
+clients = {}
+
+env = spotBezierEnv(
+    render=True,
+    on_rack=False,
+    height_field=False,
+    draw_foot_path=False,
+    env_randomizer=None,
+)
+gui = GUI(env.spot.quadruped)
+bodyState = BodyState()
+gaitState = GaitState()
+spot = SpotModel()
+bezierGait = BezierGait()
+simulator = Simulator()
+
+class Command(Enum):
+    ESTOP = 0
+    CONTROLLER = 1
+
+
+def get_controller(buffer):
+    buffer = struct.unpack("<8b", buffer)
+    return {
+        "command": buffer[0],
+        "estop": buffer[1],
+        "lx": buffer[2],
+        "ly": buffer[3],
+        "rx": buffer[4],
+        "ry": buffer[5],
+        "height": buffer[6],
+        "speed": buffer[7],
+    }
+
+
+async def handle_binary_message(client, data):
+    message = get_controller(data)
+    command = Command(message["command"])
+    if command == Command.ESTOP:
+        client["model"]["running"] = False
+        await client["websocket"].send(
+            json.dumps({"type": "stop", "data": "Servos stopped"})
+        )
+
+    if command == Command.CONTROLLER:
+        await client["websocket"].send(json.dumps({"type": "echo", "data": message}))
+
+
+async def handle_json_message(client, message):
+    data = json.loads(message)
+    client = client["clientState"]
+    if data["type"] in ("stop", "mode_change"):
+        client["model"][data["type"]] = data.get("data", False)
+        await client["websocket"].send(
+            json.dumps(
+                {"type": data["type"], "data": data.get("data", "Servos stopped")}
+            )
+        )
+
+
+async def handle_message(websocket, path):
+    client_id = id(websocket)
+    clients[client_id] = {
+        "clientState": model(),
+        "websocket": websocket,
+    }
+    try:
+        async for message in websocket:
+            if isinstance(message, bytes):
+                await handle_binary_message(clients[client_id], message)
+            else:
+                await handle_json_message(clients[client_id], message)
+    finally:
+        del clients[client_id]
+
+
+async def main():
+    async with websockets.serve(handle_message, "localhost", 2096):
+        print("Server starting")
+        await asyncio.Future()
+
+
+if __name__ == "__main__":
+    asyncio.run(main())

mock/simulator/.gitignore

@@ -0,0 +1,5 @@
+/venv/
+/.idea/
+*.egg-info
+*.DS_Store
+/dist