chore: dev

.gitignore

@@ -0,0 +1,3 @@
+.venv
+3rdparty
+

README.md

@@ -1 +0,0 @@
-test

logging_config.yaml

@@ -0,0 +1,43 @@
+version: 1
+disable_existing_loggers: false
+formatters:
+  standard:
+    format: '[%(levelname)s] %(asctime)s [%(name)s:%(lineno)d]:%(message)s'
+handlers:
+  access:
+    class: logging.StreamHandler
+    formatter: standard
+    stream: ext://sys.stdout
+  default:
+    class: logging.StreamHandler
+    formatter: standard
+    stream: ext://sys.stderr
+  console:
+    level: INFO
+    class: logging.StreamHandler
+    formatter: standard
+  timed_rotating_file:
+    class: logging.handlers.TimedRotatingFileHandler
+    filename: logs/app.log
+    when: 'midnight'
+    interval: 1
+    backupCount: 7
+    level: INFO
+    formatter: standard
+    encoding: utf-8
+loggers:
+  '':
+    handlers: [console, timed_rotating_file]
+    level: DEBUG
+    propagate: true
+  uvicorn:
+    handlers: [default, timed_rotating_file]
+    level: INFO
+    propagate: false
+  uvicorn.access:
+    handlers: [access, timed_rotating_file]
+    level: INFO
+    propagate: false
+  uvicorn.error:
+    handlers: [console, timed_rotating_file]
+    level: INFO

motor_instance.py

@@ -0,0 +1,44 @@
+import time
+
+from unitree_actuator_sdk import *
+
+
+class MotorInstance(object):
+    motor_name: str
+    serial_path: str
+    id: int
+    motor_mode: MotorMode
+    motor_type: MotorType
+    serial: SerialPort
+
+    motor_cmd: MotorCmd
+    motor_data: MotorData
+
+    def __init__(
+        self,
+        serial_path: str,
+        id: int,
+        motor_name: str = None,
+        mode: MotorMode = MotorMode.CALIBRATE,
+        motor_type: MotorType = MotorType.A1,
+    ):
+        self.serial_path = serial_path
+        self.id = id
+        self.motor_mode = mode
+        self.motor_type = motor_type
+        self.motor_name = "-".join([self.serial_path, str(id), str(int(time.time()))]) if motor_name is None else motor_name
+        self.serial = SerialPort(self.serial_path)
+
+    def reset(self):
+        pass
+
+    def send_pingpong(self):
+        pass
+
+    def get_motor_name(self):
+        return self.motor_name
+
+    def sendrecv(self, cmd: MotorCmd) -> MotorData:
+        data: MotorData = MotorData()
+        self.serial.sendRecv(cmd, data)
+        return data

motor_manager.py

@@ -0,0 +1,81 @@
+import time
+import threading
+import traceback
+import random
+import os
+import logging
+import yaml
+import typing
+
+from unitree_actuator_sdk import *
+
+from motor_instance import MotorInstance
+
+base_dir = os.path.dirname(__file__)
+log_config = None
+if not os.path.exists(base_dir + "/logs"):
+    os.mkdir(base_dir + "/logs")
+with open(base_dir + "/logging_config.yaml", "r") as f:
+    log_config = yaml.safe_load(f.read())
+    logging.config.dictConfig(log_config)
+
+logger = logging.getLogger(__file__.split("/")[-1])
+
+for handle in logger.handlers:
+    if isinstance(handle, logging.handlers.TimedRotatingFileHandler):
+        handle.suffix = "%Y-%m-%d.log"
+
+
+class MotorManager(object):
+    transfer_thread: threading.Thread | None = None
+    cmd_interval_ms: int
+    motor_dict: dict[MotorInstance] = dict()
+    loop_flag: bool = False
+
+    motor_cmds: dict[str, int]
+
+    task_list: dict[str, typing.Callable] = {}
+
+    def __init__(self, cmd_interval_ms: int):
+        self.cmd_interval_ms = cmd_interval_ms
+
+    def register_motor(self, motor: MotorInstance):
+        self.motor_dict[motor.get_motor_name()] = motor
+
+    def register_task(self, task: typing.Callable, task_name: str = None) -> str:
+        if task_name is None or task_name == "":
+            task_name = task.__name__ + "-" + str(time.time * 1000)
+        self.task_list[task_name] = task
+        return task_name
+
+    def run(self):
+        self.loop_flag = True
+        self.transfer_thread = threading.Thread(target=self.loop)
+        self.transfer_thread.start()
+
+    def stop(self):
+        self.stop_without_join()
+        self.transfer_thread.join()
+
+    def stop_without_join(self):
+        self.loop_flag = False
+
+    def loop(self):
+        cur_time = time.time() * 1000
+        next_run_time = cur_time + self.cmd_interval_ms
+        while self.loop_flag:
+            for task_name, task in self.task_list.items():
+                try:
+                    task(self, task_name)
+                except Exception as e:
+                    logger.warning(f"run task: {task_name} has trouble: {traceback.format_exc()}")
+            cur_time = time.time() * 1000
+            time_delta = cur_time - next_run_time
+            if time_delta < 0:
+                logger.warning(f"loop run too slow, took {self.cmd_interval_ms - time_delta} ms")
+                next_run_time = cur_time + self.cmd_interval_ms
+                # timeout and no sleep
+            else:
+                sleep_seconds = (next_run_time - cur_time) / 1000
+                next_run_time = next_run_time + self.cmd_interval_ms
+                time.sleep(sleep_seconds)

requirements.txt

@@ -0,0 +1 @@
+pyyaml

unitree_actuator_sdk.py

@@ -0,0 +1,49 @@
+# Mock sdk interface for development
+
+import enum
+
+
+class MotorType(enum.Enum):
+    A1 = enum.auto()
+    B1 = enum.auto()
+    GO_M8010_6 = enum.auto()
+
+
+class MotorMode(enum.Enum):
+    BRAKE = enum.auto()
+    FOC = enum.auto()
+    CALIBRATE = enum.auto()
+
+
+class MotorCmd(object):
+    motorType: MotorType
+    hex_len: int
+    id: int
+    mode: int
+    tau: float
+    dq: float
+    q: float
+    kp: float
+    kd: float
+
+
+class MotorData(object):
+    motorType: MotorType
+    hex_len: int
+    motor_id: int
+    mode: int
+    temp: int
+    merror: int
+    tau: float
+    dq: float
+    q: float
+    correct: bool
+
+
+class SerialPort(object):
+    def test(self): ...
+    def sendRecv(cmd: MotorCmd, data: MotorData) -> bool: ...
+
+
+def queryMotorMode(motortype: MotorType, motormode: MotorMode): ...
+def queryGearRatio(motortype: MotorType): ...

utils.py

@@ -0,0 +1,61 @@
+import time
+import logging
+import asyncio
+
+from functools import wraps
+
+logger = logging.getLogger(__file__.split("/")[-1])
+
+
+__LIMIT_TIMEIT_PRINT_MAX_LEN__ = 128
+
+
+def timeit(
+    func=None, *, level: int = logging.DEBUG, output_args: bool = False, output_maxlen: int = __LIMIT_TIMEIT_PRINT_MAX_LEN__
+):
+    if func is not None:
+        return timeit(level=level, output_args=output_args, output_maxlen=output_maxlen)(func)
+
+    def _get_timeit_arg_print(*args, **kwargs):
+        args_print = f"{args}"
+        args_print = args_print if len(args_print) < output_maxlen else args_print[: output_maxlen - 4] + "...)"
+        kwargs_print = f"{kwargs}"
+        kwargs_print = kwargs_print if len(kwargs_print) < output_maxlen else kwargs_print[: output_maxlen - 4] + "...}"
+        return args_print, kwargs_print
+
+    def _decorator(func):
+        if logger.level > level:
+            return func
+        is_async = asyncio.iscoroutinefunction(func)
+
+        @wraps(func)
+        async def _async_timeit_wrapper(*args, **kwargs):
+            if output_args:
+                args_print, kwargs_print = _get_timeit_arg_print(args, kwargs)
+                logger.log(level, f"async call {func.__name__} {args_print} {kwargs_print}")
+            else:
+                logger.log(level, f"async call {func.__name__}")
+            start_time = time.perf_counter()
+            result = await func(*args, **kwargs)
+            end_time = time.perf_counter()
+            total_time = end_time - start_time
+            logger.log(level, f"async quit {func.__name__} Took {total_time:.3f} seconds")
+            return result
+
+        @wraps(func)
+        def _sync_timeit_wrapper(*args, **kwargs):
+            if output_args:
+                args_print, kwargs_print = _get_timeit_arg_print(args, kwargs)
+                logger.log(level, f"sync call {func.__name__} {args_print} {kwargs_print}")
+            else:
+                logger.log(level, f"sync call {func.__name__}")
+            start_time = time.perf_counter()
+            result = func(*args, **kwargs)
+            end_time = time.perf_counter()
+            total_time = end_time - start_time
+            logger.log(level, f"sync quit {func.__name__} Took {total_time:.3f} seconds")
+            return result
+
+        return _async_timeit_wrapper if is_async else _sync_timeit_wrapper
+
+    return _decorator