pytmrobot 0.1.0__py3-none-any.whl

pytmrobot/__init__.py

@@ -0,0 +1,89 @@
+"""
+pytmrobot — Python TM Robot 控制函式庫
+
+讓 Python 開發者無需了解 TMScript，直接以 Pythonic API 控制達明機器人。
+
+Quick Start::
+
+    from pytmrobot import TMRobot, CartPoint
+
+    with TMRobot("192.168.1.101") as robot:
+        home = CartPoint(400, -100, 500, 180, 0, 90)
+        pick = CartPoint(400, -100, 280, 180, 0, 90)
+
+        robot.move.ptp(home, speed=30)
+        robot.move.ptp(pick, speed=10)
+        robot.io.set_do(2, True)          # 夾爪閉合
+        robot.sleep(500)
+        robot.move.ptp(home, speed=30)
+        robot.io.set_do(2, False)         # 夾爪張開
+
+        pos = robot.state.tcp_position
+        print(f"完成！位置: X={pos[0]:.2f}, Y={pos[1]:.2f}, Z={pos[2]:.2f}")
+
+Public API
+----------
+主類別：
+    TMRobot         控制機器人的主要入口（支援 context manager）
+
+點位型別：
+    CartPoint       笛卡爾座標目標點 (X, Y, Z, Rx, Ry, Rz)
+    JointPoint      關節角度目標點 (J1, J2, J3, J4, J5, J6)
+
+列舉：
+    SpeedUnit       直線/圓弧速度單位 (PERCENT / ABS_SYNC / ABS_LOCK)
+    BlendUnit       軌跡混合單位 (PERCENT / RADIUS)
+
+資料容器：
+    RobotState      robot.state.snapshot() 的回傳型別
+    IOState         robot.io.get_all_io() 的回傳型別
+
+例外：
+    TMError         所有例外的基礎類別
+    TMConnectionError  連線失敗
+    TMScriptError   機器人回傳 ERROR
+    TMTimeoutError  等待逾時
+    TMValueError    參數超出範圍
+"""
+
+from .exceptions import (
+    TMConnectionError,
+    TMError,
+    TMScriptError,
+    TMTimeoutError,
+    TMValueError,
+)
+from .robot import TMRobot
+from .types import (
+    BlendUnit,
+    CartPoint,
+    IOState,
+    JointPoint,
+    Pose6,
+    RobotState,
+    SpeedUnit,
+)
+
+__version__ = "0.1.0"
+__author__  = "TMDemo Contributors"
+
+__all__ = [
+    # 主類別
+    "TMRobot",
+    # 點位型別
+    "CartPoint",
+    "JointPoint",
+    "Pose6",
+    # 列舉
+    "SpeedUnit",
+    "BlendUnit",
+    # 資料容器
+    "RobotState",
+    "IOState",
+    # 例外
+    "TMError",
+    "TMConnectionError",
+    "TMScriptError",
+    "TMTimeoutError",
+    "TMValueError",
+]

pytmrobot/_client.py

@@ -0,0 +1,161 @@
+"""
+pytmrobot._client — TMClientWrapper
+
+薄包裝現有 TMClient，新增 send_and_recv_subcmd() 實現狀態讀取。
+不修改 tm_listen_client.py 任何一行。
+"""
+
+from __future__ import annotations
+
+import time
+
+from ._protocol import TMClient, build_packet
+from .exceptions import TMConnectionError, TMScriptError, TMTimeoutError
+
+
+class TMClientWrapper:
+    """包裝 TMClient，對外提供更高階的收發介面。
+
+    主要新增 :meth:`send_and_recv_subcmd`，用於實作 ListenSend 狀態讀取。
+    其餘方法直接委派給底層 TMClient。
+    """
+
+    def __init__(self, ip: str, port: int = 5890, timeout: float = 60.0) -> None:
+        self._client = TMClient(ip=ip, port=port)
+        self._timeout = timeout
+
+    # ── 連線管理（委派） ──────────────────────────────────────────────────────
+
+    def connect(self) -> None:
+        """建立 TCP 連線並等待機器人進入 Listen Node。"""
+        try:
+            self._client.connect()
+            self._client.wait_for_listen_node()
+        except OSError as e:
+            raise TMConnectionError(str(e)) from e
+
+    def disconnect(self) -> None:
+        """關閉 TCP 連線。"""
+        self._client.disconnect()
+
+    # ── 一般 script 發送（委派） ──────────────────────────────────────────────
+
+    def send_script(self, script_id: str, lines: list[str]) -> bool:
+        """發送 TMSCT，等待 OK/ERROR 回應（委派 TMClient.send_script）。
+
+        Returns:
+            True=OK, False=ERROR 或無回應
+        """
+        return self._client.send_script(script_id, lines)
+
+    def send_script_checked(self, script_id: str, lines: list[str]) -> None:
+        """發送 TMSCT，失敗時拋出 TMScriptError。"""
+        ok = self.send_script(script_id, lines)
+        if not ok:
+            raise TMScriptError(f"指令執行失敗：{lines}", script_id=script_id)
+
+    def wait_queue_tag(self, tag_id: int, poll_interval: float = 0.2) -> None:
+        """等待 QueueTag 完成（委派 TMClient.wait_queue_tag）。"""
+        self._client.wait_queue_tag(tag_id, poll_interval)
+
+    def exit_listen(self, pass_exit: bool = True) -> None:
+        """退出 Listen Node（委派 TMClient.exit_listen）。"""
+        self._client.exit_listen(pass_exit)
+
+    # ── 狀態讀取（ListenSend 往返） ───────────────────────────────────────────
+
+    def send_and_recv_subcmd(
+        self,
+        script_id: str,
+        lines: list[str],
+        expected_subcmd: int = 90,
+        timeout: float = 5.0,
+    ) -> str:
+        """發送含 ListenSend 的 script，等待 TMSCT OK 與 TMSTA SubCmd 回傳。
+
+        使用狀態機接收迴圈，不假設封包到達順序：
+        - TMSCT OK  → ok_received = True
+        - TMSCT ERROR / CPERR → 拋出 TMScriptError
+        - TMSTA subcmd == expected_subcmd → data_received = csv_string
+        - TMSTA 其他 → 忽略（殘留 QueueTag 通知等）
+        - ok_received AND data_received → 回傳 csv_string
+        - 逾時 → 拋出 TMTimeoutError
+
+        Args:
+            script_id:       TMSCT 的 Script ID
+            lines:           TMScript 行列表（最後一行應含 ListenSend）
+            expected_subcmd: 期望的 TMSTA SubCmd（90-99）
+            timeout:         等待逾時秒數
+
+        Returns:
+            TMSTA 回傳的資料字串（SubCmd 之後的部分）
+        """
+        # 發送封包
+        pkt = build_packet("TMSCT", f"{script_id}," + "\r\n".join(lines))
+        self._client.send_raw(pkt)
+
+        ok_received   = False
+        data_received: str | None = None
+        deadline      = time.monotonic() + timeout
+        orig_timeout  = self._client.sock.gettimeout() if self._client.sock else timeout
+
+        while time.monotonic() < deadline:
+            # 設短的 socket timeout 避免長時間阻塞
+            if self._client.sock:
+                self._client.sock.settimeout(min(0.5, deadline - time.monotonic()))
+
+            try:
+                pkt_dict = self._client.recv_packet()
+            except TimeoutError:
+                continue
+            except Exception as e:
+                raise TMConnectionError(str(e)) from e
+            finally:
+                if self._client.sock:
+                    try:
+                        self._client.sock.settimeout(orig_timeout)
+                    except Exception:
+                        pass
+
+            if pkt_dict is None:
+                continue
+
+            hdr  = pkt_dict["header"]
+            data = pkt_dict["data"]
+
+            if hdr == "TMSCT":
+                parts  = data.split(",", 1)
+                status = parts[1] if len(parts) > 1 else ""
+                if status.startswith("OK"):
+                    ok_received = True
+                else:
+                    raise TMScriptError(status, script_id=script_id)
+
+            elif hdr == "TMSTA":
+                parts = data.split(",", 1)
+                subcmd_str = parts[0].strip()
+                try:
+                    subcmd = int(subcmd_str)
+                except ValueError:
+                    continue
+                if subcmd == expected_subcmd:
+                    # data 欄位: "<subcmd>,<payload>"
+                    data_received = parts[1] if len(parts) > 1 else ""
+
+            elif hdr == "CPERR":
+                raise TMScriptError(f"CPERR: {data}", script_id=script_id)
+
+            if ok_received and data_received is not None:
+                return data_received
+
+        raise TMTimeoutError(
+            f"等待 TMSTA SubCmd {expected_subcmd} 逾時（{timeout}s），"
+            f"ok={ok_received}, data={data_received!r}"
+        )
+
+    # ── 底層存取 ──────────────────────────────────────────────────────────────
+
+    @property
+    def raw_client(self) -> TMClient:
+        """取得底層 TMClient 實例（進階使用）。"""
+        return self._client

pytmrobot/_ft_base.py

@@ -0,0 +1,115 @@
+"""
+pytmrobot._ft_base — Compliance / TouchStop / Force 共用基底
+
+三個控制器共享相同的「宣告 → 設定 → Start() → ListenSend」模式，
+以及 Listen Node session 變數宣告追蹤邏輯，在此統一實作。
+"""
+
+from __future__ import annotations
+
+from typing import TYPE_CHECKING
+
+from .types import Pose6
+
+if TYPE_CHECKING:
+    from ._client import TMClientWrapper
+    from .tag_manager import TagManager
+
+
+class _FTBase:
+    """Compliance / TouchStop / Force 共用基底類別。
+
+    子類別需定義：
+    - ``_OBJ_NAME``:   TMScript 物件變數名稱，如 ``"_ts"``
+    - ``_RESULT_VAR``: TMScript 整數結果變數名稱，如 ``"_ts_r"``
+
+    **Start() 傳回碼（int）：**
+
+    +------+----------------------------------+
+    |  0   | Not Working                      |
+    |  1   | Working（Start() 仍在執行中）     |
+    |  2   | Timeout                          |
+    |  3   | Distance reached（正常完成）      |
+    |  4   | IO triggered                     |
+    |  5   | Resisted（偵測到外部阻力）        |
+    |  6   | Error                            |
+    |  7   | Over Speed                       |
+    |  8   | Digital IO triggered             |
+    |  9   | Analog IO triggered              |
+    | 10   | Variable                         |
+    | 11   | Force is Satisfied               |
+    | 12   | Allowable Position Tolerances    |
+    | 13   | Motion Finish（軌跡結束正常完成） |
+    +------+----------------------------------+
+    """
+
+    _OBJ_NAME:   str = ""
+    _RESULT_VAR: str = ""
+
+    def __init__(self, client: TMClientWrapper, tags: TagManager) -> None:
+        self._client   = client
+        self._tags     = tags
+        self._declared: dict[str, bool] = {}
+
+    def _reset_session(self) -> None:
+        """重設所有 TMScript 變數宣告狀態（重新連線時呼叫）。"""
+        self._declared.clear()
+
+    # ── 內部工具 ──────────────────────────────────────────────────────────────
+
+    def _is_declared(self, var: str) -> bool:
+        return self._declared.get(var, False)
+
+    def _mark(self, var: str) -> None:
+        self._declared[var] = True
+
+    def _assign(self, tmtype: str, var: str, expr: str) -> str:
+        """依宣告狀態回傳 'TYPE var = expr' 或 'var = expr'。"""
+        if not self._is_declared(var):
+            self._mark(var)
+            return f"{tmtype} {var} = {expr}"
+        return f"{var} = {expr}"
+
+    def _sid(self, suffix: str = "") -> str:
+        """生成腳本 ID（使用物件名去掉底線為前綴）。"""
+        tag = self._tags.next()
+        prefix = self._OBJ_NAME.lstrip("_")
+        return f"{prefix}{suffix}{tag}"
+
+    # ── 核心執行 ──────────────────────────────────────────────────────────────
+
+    def _run(
+        self,
+        setup_lines: list[str],
+        start_expr:  str,
+        timeout_ms:  int,
+    ) -> int:
+        """發送設定 + Start() 腳本，以 ListenSend SubCmd 90 接收結果碼。
+
+        Args:
+            setup_lines: 物件宣告/設定行（Reset, Frame, Single, Timeout…）
+            start_expr:  Start() 呼叫字串，如 ``"_ts.Start()"``
+            timeout_ms:  TMScript 逾時設定，用於計算 Python 端等待上限
+        """
+        result_line = self._assign("int", self._RESULT_VAR, start_expr)
+        lines = setup_lines + [result_line,
+                               f"ListenSend(90, GetString({self._RESULT_VAR}))"]
+        py_timeout = (timeout_ms / 1000.0 + 5.0) if timeout_ms > 0 else 3600.0
+        csv = self._client.send_and_recv_subcmd(self._sid(), lines, timeout=py_timeout)
+        return int(csv.strip())
+
+    def _stop(self) -> int:
+        """發送 Stop() 腳本並以 ListenSend 接收結果碼。"""
+        result_line = self._assign("int", self._RESULT_VAR,
+                                   f"{self._OBJ_NAME}.Stop()")
+        lines = [result_line, f"ListenSend(90, GetString({self._RESULT_VAR}))"]
+        csv = self._client.send_and_recv_subcmd(self._sid("stop"), lines)
+        return int(csv.strip())
+
+    def _read_pos(self, method_call: str, var_name: str) -> Pose6:
+        """發送 GetXxxPos(type) → ListenSend，解析 float[] 座標為 Pose6。"""
+        assign = self._assign("float[]", var_name, method_call)
+        lines  = [assign, f"ListenSend(90, GetString({var_name}))"]
+        csv    = self._client.send_and_recv_subcmd(self._sid("pos"), lines)
+        parts  = [v.strip() for v in csv.strip().strip("{}").split(",") if v.strip()]
+        return tuple(float(p) for p in parts)  # type: ignore[return-value]

pytmrobot/_protocol.py

@@ -0,0 +1,171 @@
+"""
+pytmrobot._protocol — TM Robot Listen Node 底層通訊協定。
+
+包含封包編解碼與 TMClient 類別。
+原始碼源自 tm_listen_client.py（同 repo），
+抽出為獨立私有模組以支援 PyPI 打包安裝。
+"""
+
+from __future__ import annotations
+
+import re
+import socket
+import time
+
+# ── 封包工具 ─────────────────────────────────────────────────────────────────
+
+
+def calc_checksum(raw: str) -> str:
+    """計算封包 checksum：對 $ 與 * 之間的所有位元組做 XOR。
+
+    raw 為完整字串，含頭尾的 $ 與 *，但不含 \\r\\n。
+    例如 raw = "$TMSCT,5,10,OK,*"
+
+    使用 rindex 找最後一個 *（checksum 分隔符），
+    避免 TMScript 腳本內容含乘法符號 * 時計算出錯（CPERR 02）。
+    """
+    inner = raw[1 : raw.rindex("*")]
+    cs = 0
+    for ch in inner.encode("utf-8"):
+        cs ^= ch
+    return format(cs, "02X")
+
+
+def build_packet(header: str, data: str) -> bytes:
+    """建立封包並計算 checksum。
+
+    Args:
+        header: ``'TMSCT'`` | ``'TMSTA'``
+        data:   資料欄位字串（不含 checksum）
+    """
+    length = len(data.encode("utf-8"))
+    raw = f"${header},{length},{data},*"
+    cs = calc_checksum(raw)
+    packet = f"{raw}{cs}\r\n"
+    return packet.encode("utf-8")
+
+
+def parse_packet(line: str) -> dict | None:
+    """解析一行機器人回覆，回傳 dict 或 None（格式錯誤時）。"""
+    line = line.strip()
+    if not line.startswith("$"):
+        return None
+    m = re.match(r"^\$([^,]+),(\d+),(.+),\*([0-9A-Fa-f]{2})$", line)
+    if not m:
+        return None
+    header, length, data, cs = m.groups()
+    return {"header": header, "length": int(length), "data": data, "cs": cs}
+
+
+# ── TMClient ─────────────────────────────────────────────────────────────────
+
+_DEFAULT_TIMEOUT = 60.0
+
+
+class TMClient:
+    """TM Robot Listen Node TCP 客戶端。
+
+    提供底層 send/recv 與常用操作（等待 Listen Node、送指令、等 QueueTag）。
+    """
+
+    def __init__(self, ip: str, port: int = 5890, timeout: float = _DEFAULT_TIMEOUT):
+        self.ip = ip
+        self.port = port
+        self.sock: socket.socket | None = None
+        self._buf = ""
+
+    # ── 連線 / 斷線 ──────────────────────────────────────────────────────────
+
+    def connect(self) -> None:
+        self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        self.sock.settimeout(_DEFAULT_TIMEOUT)
+        self.sock.connect((self.ip, self.port))
+
+    def disconnect(self) -> None:
+        if self.sock:
+            self.sock.close()
+            self.sock = None
+
+    # ── 底層收發 ─────────────────────────────────────────────────────────────
+
+    def _recv_line(self) -> str:
+        """從緩衝區讀取一行（以 \\r\\n 結尾）。"""
+        assert self.sock is not None
+        while "\r\n" not in self._buf:
+            chunk = self.sock.recv(4096).decode("utf-8", errors="replace")
+            if not chunk:
+                raise ConnectionError("連線中斷")
+            self._buf += chunk
+        line, self._buf = self._buf.split("\r\n", 1)
+        return line
+
+    def recv_packet(self) -> dict | None:
+        """接收並解析一個封包。"""
+        line = self._recv_line()
+        return parse_packet(line)
+
+    def send_raw(self, packet: bytes) -> None:
+        assert self.sock is not None
+        self.sock.sendall(packet)
+
+    # ── 等待 Listen Node ─────────────────────────────────────────────────────
+
+    def wait_for_listen_node(self) -> None:
+        """確保機器人已進入 Listen Node。"""
+        if self.query_listen_mode():
+            return
+        while True:
+            pkt = self.recv_packet()
+            if pkt and pkt["header"] == "TMSCT":
+                return
+
+    def query_listen_mode(self) -> bool:
+        """SubCmd 00：查詢是否已進入外部 Script 控制模式。"""
+        pkt = build_packet("TMSTA", "00")
+        self.send_raw(pkt)
+        resp = self.recv_packet()
+        if resp and resp["header"] == "TMSTA":
+            return resp["data"].split(",")[1].lower() == "true"
+        return False
+
+    # ── 發送 TMScript 指令 ────────────────────────────────────────────────────
+
+    def send_script(self, script_id: str, script_lines: list[str]) -> bool:
+        """發送一或多行 TMScript，等待 OK/ERROR 回應。"""
+        data = f"{script_id}," + "\r\n".join(script_lines)
+        pkt = build_packet("TMSCT", data)
+        self.send_raw(pkt)
+
+        while True:
+            resp = self.recv_packet()
+            if not resp:
+                return False
+            if resp["header"] == "TMSTA":
+                continue
+            if resp["header"] == "TMSCT":
+                parts = resp["data"].split(",", 1)
+                status = parts[1] if len(parts) > 1 else ""
+                return status.startswith("OK")
+            if resp["header"] == "CPERR":
+                return False
+
+    # ── 等待 QueueTag ─────────────────────────────────────────────────────────
+
+    def wait_queue_tag(self, tag_id: int, poll_interval: float = 0.2) -> None:
+        """等待機器人完成指定的 QueueTag（輪詢）。"""
+        while True:
+            pkt = build_packet("TMSTA", f"01,{tag_id}")
+            self.send_raw(pkt)
+            resp = self.recv_packet()
+            if resp and resp["header"] == "TMSTA":
+                parts = resp["data"].split(",")
+                if len(parts) >= 3 and parts[2].lower() == "true":
+                    return
+            time.sleep(poll_interval)
+
+    # ── 退出 Listen Node ──────────────────────────────────────────────────────
+
+    def exit_listen(self, pass_exit: bool = True) -> None:
+        """發送 ScriptExit() 退出 Listen Node。"""
+        cmd = "ScriptExit(1)" if pass_exit else "ScriptExit(0)"
+        self.send_script("exit", [cmd])

pytmrobot/compliance.py

@@ -0,0 +1,92 @@
+"""
+pytmrobot.compliance — ComplianceController
+
+透過 robot.compliance 存取。使用 TM Robot 的 Compliance 功能，
+讓機器人以順從控制方式沿指定軸移動，遇到阻力時停止。
+
+TMScript 對應：Chapter 20 Compliance
+"""
+
+from __future__ import annotations
+
+from typing import TYPE_CHECKING
+
+from . import script_builder as sb
+from ._ft_base import _FTBase
+
+if TYPE_CHECKING:
+    pass
+
+
+class ComplianceController(_FTBase):
+    """Compliance 順從控制器。透過 ``robot.compliance`` 存取。
+
+    典型流程::
+
+        # 在 Z- 方向最多移動 30mm，目標力 3N，速度 5%
+        result = robot.compliance.run(
+            direction=32,    # 32 = Z-
+            distance=30.0,
+            force=3.0,
+            speed=5,
+        )
+        if result == 5:   # Resisted（偵測到外部阻力）
+            print("已接觸工件")
+        elif result == 3: # Distance reached
+            print("移動完成但未偵測到阻力")
+
+    Start() 結果碼（result 值）請參考 _FTBase docstring。
+    """
+
+    _OBJ_NAME   = "_cp"
+    _RESULT_VAR = "_cp_r"
+
+    def run(
+        self,
+        direction:  int,
+        distance:   float,
+        force:      float,
+        speed:      int,
+        *,
+        frame:           int  = 1,
+        timeout_ms:      int  = -1,
+        high_resistance: bool = False,
+    ) -> int:
+        """執行 Compliance 順從控制運動。
+
+        Args:
+            direction:       偵測軸方向代碼（TMScript ch20 Single() 參數，
+                             如 1=X+, 2=X-, 4=Y+, 8=Y-, 16=Z+, 32=Z-）
+            distance:        最大順從移動距離（mm）
+            force:           目標/阻力閾值（N）
+            speed:           運動速度（%）
+            frame:           座標系（1=機器人基座, 2=工具, 3=使用者）
+            timeout_ms:      逾時毫秒（-1=不逾時）
+            high_resistance: True=啟用高阻力偵測模式
+
+        Returns:
+            Start() 結果碼（見 _FTBase docstring）
+
+        Examples::
+
+            result = robot.compliance.run(32, 30.0, 3.0, 5)
+            if result in (3, 5):
+                print("Compliance 完成")
+        """
+        obj_declared = self._is_declared(self._OBJ_NAME)
+        setup = sb.build_compliance_setup(
+            direction, distance, force, speed,
+            frame=frame, timeout_ms=timeout_ms,
+            high_resistance=high_resistance,
+            obj_declared=obj_declared,
+        )
+        self._mark(self._OBJ_NAME)
+        return self._run(setup, f"{self._OBJ_NAME}.Start()", timeout_ms)
+
+    def stop(self) -> int:
+        """中止 Compliance 運動。
+
+        Returns:
+            Stop() 結果碼
+        """
+        return self._stop()