pylekiwi 0.1.0__tar.gz

pylekiwi-0.1.0/PKG-INFO

@@ -0,0 +1,33 @@
+Metadata-Version: 2.3
+Name: pylekiwi
+Version: 0.1.0
+Summary: Add your description here
+Author: neka-nat
+Author-email: neka-nat <nekanat.stock@gmail.com>
+Requires-Dist: eclipse-zenoh>=1.5.1
+Requires-Dist: gunicorn>=23.0.0
+Requires-Dist: kinpy>=0.5.4
+Requires-Dist: loguru>=0.7.3
+Requires-Dist: mesop>=1.1.0
+Requires-Dist: numpy>=2.3.3
+Requires-Dist: opencv-python-headless>=4.11.0.86
+Requires-Dist: pydantic>=2.11.10
+Requires-Dist: rustypot>=1.3.0
+Requires-Python: >=3.11
+Description-Content-Type: text/markdown
+
+# pylekiwi
+
+Python package for controlling the LeKiwi robot.
+
+## Quick Start
+
+Log into the robot and run the following command:
+
+```bash
+ssh <your robot ip>
+sudo chmod 666 /dev/ttyACM0
+uvx launch-lekiwi-webui --serial-port /dev/ttyACM0
+```
+
+Then, open a web browser and navigate to `http://<your robot ip>:8080` to see the web UI.

pylekiwi-0.1.0/README.md

@@ -0,0 +1,15 @@
+# pylekiwi
+
+Python package for controlling the LeKiwi robot.
+
+## Quick Start
+
+Log into the robot and run the following command:
+
+```bash
+ssh <your robot ip>
+sudo chmod 666 /dev/ttyACM0
+uvx launch-lekiwi-webui --serial-port /dev/ttyACM0
+```
+
+Then, open a web browser and navigate to `http://<your robot ip>:8080` to see the web UI.

pylekiwi-0.1.0/pyproject.toml

@@ -0,0 +1,28 @@
+[project]
+name = "pylekiwi"
+version = "0.1.0"
+description = "Add your description here"
+readme = "README.md"
+authors = [
+    { name = "neka-nat", email = "nekanat.stock@gmail.com" }
+]
+requires-python = ">=3.11"
+dependencies = [
+    "eclipse-zenoh>=1.5.1",
+    "gunicorn>=23.0.0",
+    "kinpy>=0.5.4",
+    "loguru>=0.7.3",
+    "mesop>=1.1.0",
+    "numpy>=2.3.3",
+    "opencv-python-headless>=4.11.0.86",
+    "pydantic>=2.11.10",
+    "rustypot>=1.3.0",
+]
+
+[build-system]
+requires = ["uv_build>=0.8.15,<0.9.0"]
+build-backend = "uv_build"
+
+[project.scripts]
+launch-lekiwi-host = "pylekiwi.commands.launch_host_controller:main"
+launch-lekiwi-webui = "pylekiwi.commands.launch_web_ui:main"

pylekiwi-0.1.0/src/pylekiwi/__init__.py

@@ -0,0 +1,12 @@
+from pylekiwi.arm_controller import ArmController
+from pylekiwi.base_controller import BaseController
+from pylekiwi.models import ArmJointCommand, BaseCommand
+from pylekiwi.settings import Settings
+
+__all__ = [
+    "ArmController",
+    "ArmJointCommand",
+    "BaseController",
+    "BaseCommand",
+    "Settings",
+]

pylekiwi-0.1.0/src/pylekiwi/arm_controller.py

@@ -0,0 +1,72 @@
+import os
+
+import numpy as np
+from loguru import logger
+from rustypot import Sts3215PyController
+
+from kinpy import build_serial_chain_from_mjcf
+
+from pylekiwi.models import ArmJointCommand, ArmState
+from pylekiwi.settings import Settings
+
+
+_MODEL_FILE = os.path.join(os.path.dirname(__file__), "data/SO101/so101_new_calib.xml")
+
+
+class ArmController:
+    JOINT_IDS = (1, 2, 3, 4, 5)
+    GRIPPER_ID = 6
+
+    def __init__(self, motor_controller: Sts3215PyController | Settings | None = None):
+        if motor_controller is None:
+            settings = Settings() if not isinstance(motor_controller, Settings) else motor_controller
+            motor_controller = Sts3215PyController(
+                serial_port=settings.serial_port,
+                baudrate=settings.baudrate,
+                timeout=settings.timeout,
+            )
+        elif isinstance(motor_controller, Settings):
+            motor_controller = Sts3215PyController(
+                serial_port=motor_controller.serial_port,
+                baudrate=motor_controller.baudrate,
+                timeout=motor_controller.timeout,
+            )
+        self.motor_controller = motor_controller
+
+        # self.chain = build_serial_chain_from_mjcf(
+        #     open(_MODEL_FILE, "rb").read(),
+        #     "gripper",
+        #     model_dir=os.path.dirname(_MODEL_FILE),
+        # )
+
+    def set_torque(self):
+        for i in self.JOINT_IDS:
+            self.motor_controller.write_torque_enable(i, True)
+        self.motor_controller.write_torque_enable(self.GRIPPER_ID, True)
+        all_ids = list(self.JOINT_IDS) + [self.GRIPPER_ID]
+        self.motor_controller.sync_write_maximum_acceleration(
+            all_ids, [600] * len(all_ids)
+        )
+
+    def disable_torque(self):
+        for i in self.JOINT_IDS:
+            self.motor_controller.write_torque_enable(i, False)
+        self.motor_controller.write_torque_enable(self.GRIPPER_ID, False)
+
+    def get_current_state(self) -> ArmState:
+        all_ids = list(self.JOINT_IDS) + [self.GRIPPER_ID]
+        joint_angles = self.motor_controller.sync_read_present_position(all_ids)
+        gripper_position = joint_angles[-1]
+        joint_angles = joint_angles[:-1]
+        logger.debug(f"Joint angles: {joint_angles}, Gripper position: {gripper_position}")
+        return ArmState(joint_angles=tuple(joint_angles), gripper_position=gripper_position)
+
+    def send_joint_action(self, action: ArmJointCommand):
+        target_ids = list(self.JOINT_IDS)
+        if action.gripper_position is not None:
+            target_ids += [self.GRIPPER_ID]
+        command = list(action.joint_angles)
+        if action.gripper_position is not None:
+            command += [action.gripper_position]
+        logger.debug(f"Arm command: {command}")
+        self.motor_controller.sync_write_goal_position(target_ids, command)

pylekiwi-0.1.0/src/pylekiwi/base_controller.py

@@ -0,0 +1,102 @@
+import numpy as np
+from loguru import logger
+from rustypot import Sts3215PyController
+
+from pylekiwi.models import BaseCommand
+from pylekiwi.settings import Settings
+
+
+class BaseController:
+    LEFT_WHEEL_ID = 7
+    BACK_WHEEL_ID = 8
+    RIGHT_WHEEL_ID = 9
+
+    # 角度φの定義（+x 前方を 0°、反時計回り）
+    _PHI_DEG = np.array([60.0, 180.0, 300.0])  # [LEFT, BACK, RIGHT]
+
+    def __init__(self, motor_controller: Sts3215PyController | Settings | None = None):
+        if motor_controller is None:
+            settings = Settings() if not isinstance(motor_controller, Settings) else motor_controller
+            motor_controller = Sts3215PyController(
+                serial_port=settings.serial_port,
+                baudrate=settings.baudrate,
+                timeout=settings.timeout,
+            )
+        elif isinstance(motor_controller, Settings):
+            motor_controller = Sts3215PyController(
+                serial_port=motor_controller.serial_port,
+                baudrate=motor_controller.baudrate,
+                timeout=motor_controller.timeout,
+            )
+        self.motor_controller = motor_controller
+
+    def set_torque(self):
+        for i in (self.LEFT_WHEEL_ID, self.BACK_WHEEL_ID, self.RIGHT_WHEEL_ID):
+            self.motor_controller.write_torque_enable(i, True)
+
+    def disable_torque(self):
+        for i in (self.LEFT_WHEEL_ID, self.BACK_WHEEL_ID, self.RIGHT_WHEEL_ID):
+            self.motor_controller.write_torque_enable(i, False)
+
+    @staticmethod
+    def _saturate_radps(w_radps: np.ndarray, max_radps: float = 10.0) -> np.ndarray:
+        m = np.max(np.abs(w_radps))
+        return w_radps if m <= max_radps else (w_radps * (max_radps / m))
+
+    @classmethod
+    def _body_to_wheel_radps(
+        cls,
+        vx: float,  # m/s
+        vy: float,  # m/s
+        vtheta_deg: float,  # deg/s
+        wheel_radius: float = 0.05,
+        base_radius: float = 0.125,
+        max_radps: float = 10.0,   # 速度上限（rad/s）
+    ) -> list[float]:
+        # 角速度を rad/s に
+        omega = np.deg2rad(vtheta_deg)
+        phi = np.deg2rad(cls._PHI_DEG)  # [LEFT, BACK, RIGHT] の順
+
+        # 標準形のキネマ行列（接線方向ベクトル）
+        M = np.column_stack((-np.sin(phi), np.cos(phi), np.full(3, base_radius)))
+        body = np.array([vx, vy, omega])
+
+        # 車輪接線速度 -> 角速度（rad/s）
+        wheel_radps = (M @ body) / wheel_radius
+
+        # 安全のため物理上限で正規化
+        wheel_radps = cls._saturate_radps(wheel_radps, max_radps=max_radps)
+        return wheel_radps.tolist()
+
+    @classmethod
+    def _wheel_radps_to_body(
+        cls,
+        left_wheel_radps: float,
+        back_wheel_radps: float,
+        right_wheel_radps: float,
+        wheel_radius: float = 0.05,
+        base_radius: float = 0.125,
+    ) -> BaseCommand:
+        phi = np.deg2rad(cls._PHI_DEG)  # [LEFT, BACK, RIGHT]
+        M = np.column_stack((-np.sin(phi), np.cos(phi), np.full(3, base_radius)))
+        # 角速度 -> 接線速度（m/s）
+        wheel_linear = wheel_radius * np.array([left_wheel_radps, back_wheel_radps, right_wheel_radps])
+        # 逆運動学
+        v = np.linalg.inv(M) @ wheel_linear
+        vx, vy, omega = v
+        return BaseCommand(x_vel=vx, y_vel=vy, theta_deg_vel=np.rad2deg(omega))
+
+    def send_action(self, action: BaseCommand):
+        wheel_radps = self._body_to_wheel_radps(
+            action.x_vel, action.y_vel, action.theta_deg_vel
+        )
+        logger.debug(f"Wheel radps: {wheel_radps}")
+        self.motor_controller.sync_write_goal_speed(
+            [self.LEFT_WHEEL_ID, self.BACK_WHEEL_ID, self.RIGHT_WHEEL_ID],
+            wheel_radps  # rad/s
+        )
+
+    def stop(self):
+        self.motor_controller.sync_write_goal_speed(
+            [self.LEFT_WHEEL_ID, self.BACK_WHEEL_ID, self.RIGHT_WHEEL_ID], [0.0, 0.0, 0.0]
+        )

pylekiwi-0.1.0/src/pylekiwi/commands/launch_host_controller.py

@@ -0,0 +1,16 @@
+import argparse
+
+from pylekiwi.nodes import HostControllerNode
+from pylekiwi.settings import Settings
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--serial-port", type=str, default="/dev/ttyACM0")
+    args = parser.parse_args()
+    host_controller_node = HostControllerNode(Settings(serial_port=args.serial_port))
+    host_controller_node.run()
+
+
+if __name__ == "__main__":
+    main()

pylekiwi-0.1.0/src/pylekiwi/commands/launch_web_ui.py

@@ -0,0 +1,13 @@
+import argparse
+import subprocess
+
+def main() -> int:
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--port", type=int, default=8080)
+    parser.add_argument("--serial-port", type=str, default="/dev/ttyACM0")
+    args = parser.parse_args()
+    port = args.port
+    proc = subprocess.run(
+        ["gunicorn", "--bind", f"0.0.0.0:{port}", "--env", f"LEKIWI_SERIAL_PORT={args.serial_port}", "pylekiwi.commands.web_ui:me"]
+    )
+    return proc.returncode