Moving robot code to new directory, adding linalg logic

.github/workflows/ci.yaml

@@ -38,3 +38,19 @@ jobs:
             echo "$formatted_files"
             exit 1
           fi
+  test:
+    runs-on: ubuntu-latest
+    steps:
+      - name: 🚀 Checkout
+        uses: actions/checkout@v4
+      - name: 🌿 Setup Bazel
+        uses: bazel-contrib/[email protected]
+        with:
+          bazelisk-cache: true
+          disk-cache: ${{ github.workflow }}
+          repository-cache: true
+          bazelrc: |
+            build --color=yes
+            build --show_timestamps
+      - name: 🧪 Run Tests
+        run: bazel test //src/...

BUILD.bazel

@@ -12,15 +12,15 @@ compile_pip_requirements(
 
 # Set up commands to build all images in the project
 command(
-    name = "api_v1_img",
-    command = "//src/api/v1:tarball",
+    name = "api_v2_img",
+    command = "//src/api/v2:tarball",
     visibility = ["//visibility:public"],
 )
 
 multirun(
     name = "build_all_imgs",
     commands = [
-        ":api_v1_img",
+        ":api_v2_img",
     ],
     jobs = 0,
 )

requirements.in

@@ -1,6 +1,7 @@
 asyncio
 fastapi
 jinja2
+numpy
 opencv-python
 uvicorn[standard]
 websockets

requirements_lock.txt

@@ -177,7 +177,9 @@ numpy==1.26.4 \
     --hash=sha256:edd8b5fe47dab091176d21bb6de568acdd906d1887a4584a15a9a96a1dca06ef \
     --hash=sha256:f870204a840a60da0b12273ef34f7051e98c3b5961b61b0c2c1be6dfd64fbcd3 \
     --hash=sha256:ffa75af20b44f8dba823498024771d5ac50620e6915abac414251bd971b4529f
-    # via opencv-python
+    # via
+    #   -r requirements.in
+    #   opencv-python
 opencv-python==4.9.0.80 \
     --hash=sha256:1a9f0e6267de3a1a1db0c54213d022c7c8b5b9ca4b580e80bdc58516c922c9e1 \
     --hash=sha256:3f16f08e02b2a2da44259c7cc712e779eff1dd8b55fdb0323e8cab09548086c0 \

src/api/v1/api.py

@@ -27,7 +27,9 @@
 
 motors = [
     DynamixelMotor(10, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0)),
-    DynamixelMotor(11, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0), True),
+    DynamixelMotor(11, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0)),
+    DynamixelMotor(12, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0), True),
+    DynamixelMotor(13, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0), True),
 ]
 ctrl = RobotControl("/dev/ttyUSB0", 1, motors)
 ctrl.init(1000000)

src/api/v1/robot.py

@@ -120,6 +120,9 @@ def __init__(
         self.initialized = False
 
     def __validate_command(self):
+        """
+        Run validation against the motor command
+        """
         if not self.initialized:
             raise Exception("Motor needs to be initialized before commanding")
 
@@ -182,10 +185,15 @@ def __read_bytes(self, offset: int, size: int) -> Optional[int]:
     def __get_memory_property(self, property: str) -> int:
         self.__validate_command()
 
-        value = self.__read_bytes(
-            getattr(self.address_config, property).byte_offset,
-            getattr(self.address_config, property).byte_size,
-        )
+        try:
+            config: MemorySegment = getattr(self.address_config, property)
+        except:
+            # Swallow the missing attribute error and throw a NotImplementedError for the motor instead
+            raise NotImplementedError(
+                f"FATAL | DXID: '{self.id}' | Motor does not support {property} attribute!"
+            )
+
+        value = self.__read_bytes(config.byte_offset, config.byte_size)
 
         if value is None:
             raise Exception(f"FATAL | DXID: '{self.id}' | Value could not be read!")
@@ -195,9 +203,17 @@ def __get_memory_property(self, property: str) -> int:
     def __set_memory_property(self, property: str, value: int):
         self.__validate_command()
 
+        try:
+            config: MemorySegment = getattr(self.address_config, property)
+        except:
+            # Swallow the missing attribute error and throw a NotImplementedError for the motor instead
+            raise NotImplementedError(
+                f"FATAL | DXID: '{self.id}' | Motor does not support {property} attribute!"
+            )
+
         self.__write_bytes(
-            getattr(self.address_config, property).byte_offset,
-            getattr(self.address_config, property).byte_size,
+            config.byte_offset,
+            config.byte_size,
             value,
         )
 
@@ -375,12 +391,27 @@ def set_velocity(self, command: Position2D) -> bool:
             math.pi / 2
         )
 
-        for motor in self.motors:
+        # rear left
+        motor_0_speed = min(max(clamped_y - clamped_x - command.rotation.theta, -1), 1)
+
+        # front left
+        motor_1_speed = min(max(clamped_y + clamped_x - command.rotation.theta, -1), 1)
+
+        # front right
+        motor_2_speed = min(max(clamped_y - clamped_x - command.rotation.theta, -1), 1)
+
+        # rear right
+        motor_3_speed = min(max(clamped_y + clamped_x - command.rotation.theta, -1), 1)
+
+        speeds = [motor_0_speed, motor_1_speed, motor_2_speed, motor_3_speed]
+
+        for i, motor in enumerate(self.motors):
+            speed = speeds[i]
             # TODO actual kinematics here
-            if clamped_y < 0:
-                motor.set_moving_speed((abs(clamped_y) * 1023) + 1023)
+            if speed < 0:
+                motor.set_moving_speed((abs(speed) * 1023) + 1023)
             else:
-                motor.set_moving_speed(clamped_y * 1023)
+                motor.set_moving_speed(speed * 1023)
 
         return True
 

src/api/v2/api.py

@@ -1,27 +1,41 @@
-from typing import Union
 from fastapi import FastAPI, Request, WebSocket, WebSocketDisconnect
 from websockets.exceptions import ConnectionClosed
 from fastapi.templating import Jinja2Templates
+from fastapi.middleware.cors import CORSMiddleware
+from robot import (
+    Position2D,
+    RobotControl,
+    DynamixelMotor,
+    DYNAMIXEL_MX_12_ADDR_CONFIG,
+)
 import asyncio
 import cv2
 
 app = FastAPI()
-camera = cv2.VideoCapture(0, cv2.CAP_DSHOW)
+camera = cv2.VideoCapture("/dev/video2")
 templates = Jinja2Templates(directory="templates")
 
+origins = [
+    "http://localhost:3000",
+]
 
-@app.get("/")
-def read_root():
-    return {"Hello": "World"}
+app.add_middleware(
+    CORSMiddleware,
+    allow_origins=origins,
+    allow_credentials=True,
+    allow_methods=["*"],  # Allow all HTTP methods
+    allow_headers=["*"],  # Allow all headers
+)
 
-
-@app.get("/items/{item_id}")
-def read_item(item_id: int, q: Union[str, None] = None):
-    return {"item_id": item_id, "q": q}
+motors = [
+    DynamixelMotor(10, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0)),
+    DynamixelMotor(11, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0), True),
+]
+ctrl = RobotControl("/dev/ttyUSB0", 1, motors)
+ctrl.init(1000000)
 
 
 # https://stackoverflow.com/a/70626324
-# Not actually sure if this works with bazel
 @app.websocket("/ws")
 async def get_stream(websocket: WebSocket):
     await websocket.accept()
@@ -36,3 +50,15 @@ async def get_stream(websocket: WebSocket):
             await asyncio.sleep(0.03)
     except (WebSocketDisconnect, ConnectionClosed):
         print("Client disconnected")
+
+
+@app.post("/command")
+async def command(request: Request):
+    data = await request.json()
+
+    x_val = -data.get("left", 0) + data.get("right", 0)
+    y_val = -data.get("down", 0) + data.get("up", 0)
+
+    ctrl.set_velocity(Position2D(x_val, y_val, 0))
+
+    return {"status": "success"}

src/robot/v1/BUILD.bazel

@@ -0,0 +1,18 @@
+load("@rules_python//python:defs.bzl", "py_library")
+
+# Put all robot files in this library
+py_library(
+    name = "robot_src",
+    srcs = [
+        "config.py",
+        "robot.py",
+        "structs.py",
+    ],
+    deps = [
+        "@pypi//fastapi:pkg",
+        "@pypi//jinja2:pkg",
+        "@pypi//numpy:pkg",
+        "@pypi//opencv_python:pkg",
+        "@pypi//websockets:pkg",
+    ],
+)

src/robot/v1/config.py

@@ -0,0 +1,24 @@
+from structs import DynamixelMotorAddressConfig, MemorySegment
+
+# Add all Dynamixel Motor Configs here
+DYNAMIXEL_MX_12_ADDR_CONFIG = DynamixelMotorAddressConfig(
+    torque_enable=MemorySegment(24, 1),
+    led_enable=MemorySegment(25, 1),
+    d_gain=MemorySegment(26, 1),
+    i_gain=MemorySegment(27, 1),
+    p_gain=MemorySegment(28, 1),
+    goal_position=MemorySegment(30, 2),
+    moving_speed=MemorySegment(32, 2),
+    torque_limit=MemorySegment(34, 2),
+    present_position=MemorySegment(36, 2),
+    present_speed=MemorySegment(38, 2),
+    present_load=MemorySegment(40, 2),
+    present_input_voltage=MemorySegment(42, 1),
+    present_temperature=MemorySegment(43, 1),
+    registered=MemorySegment(44, 1),
+    moving=MemorySegment(46, 1),
+    lock=MemorySegment(47, 1),
+    punch=MemorySegment(48, 2),
+    realtime_tick=MemorySegment(50, 2),
+    goal_acceleration=MemorySegment(73, 1),
+)