litegrip-python 1.1.2__py3-none-any.whl

litegrip/can/transport.py

@@ -0,0 +1,365 @@
+"""Raw SocketCAN transport — no dependencies beyond Python stdlib + Linux SocketCAN.
+
+Supports classic CAN (MTU 16) and CAN FD (MTU 72) with automatic mode
+detection.  Provides send/receive with optional timeout.
+"""
+
+from __future__ import annotations
+
+import fcntl
+import logging
+import select
+import socket
+import struct
+import time
+from dataclasses import dataclass
+from enum import IntEnum
+from typing import Optional
+
+log = logging.getLogger("litegrip.can.transport")
+
+# ── SocketCAN constants ──────────────────────────────────────────────────
+CAN_RAW = getattr(socket, "CAN_RAW", 1)
+CAN_RAW_FD_FRAMES = getattr(socket, "CAN_RAW_FD_FRAMES", 5)
+CAN_RAW_FILTER = getattr(socket, "CAN_RAW_FILTER", 1)
+SOL_CAN_RAW = getattr(socket, "SOL_CAN_RAW", 101)
+CAN_SFF_MASK = 0x7FF
+CAN_EFF_MASK = 0x1FFFFFFF
+
+# struct can_filter { canid_t can_id; canid_t can_mask; } — two native uint32.
+_CAN_FILTER_FMT = "=II"
+
+# Classic CAN frame: can_id(4B) + dlc(1B) + padding(3B) + data(8B) = 16B
+CAN_FRAME_FMT = "=IB3x8s"
+CAN_MTU = 16
+
+# CAN FD frame: can_id(4B) + dlc(1B) + flags(1B) + len8_flag(1B) + reserved(1B) + data(64B) = 72B
+CANFD_FRAME_FMT = "=IBBBB64s"
+CANFD_FRAME_SIZE = struct.calcsize(CANFD_FRAME_FMT)
+CANFD_BRS = 0x01
+CANFD_MTU = 72
+
+_SIOCGIFMTU = 0x8921
+
+# sizeof(struct ifreq) — 40 bytes on 64-bit Linux, 32 on 32-bit.  SIOCGIFMTU
+# writes back the whole struct, not just ifr_name plus ifr_mtu.
+_IFREQ_SIZE = 40
+
+
+def _iface_mtu(sock: socket.socket, iface: str) -> Optional[int]:
+    """Read interface MTU (16=classic CAN, 72=CAN FD). Returns None on failure."""
+    try:
+        ifr = struct.pack("16sH", iface.encode("utf-8")[:15], 0)
+        # Hand over a whole struct ifreq: the kernel writes all of it back, and
+        # since 3.14 fcntl.ioctl() raises SystemError (not OSError) when
+        # anything lands past the end of the argument.  The reply is exactly as
+        # long as the argument, so slice the mtu back out of the front.
+        ifr = ifr.ljust(_IFREQ_SIZE, b"\x00")
+        res = fcntl.ioctl(sock.fileno(), _SIOCGIFMTU, ifr)
+        return struct.unpack("16sH", res[:18])[1]
+    except OSError:
+        return None
+
+
+class CanMode(IntEnum):
+    """CAN interface mode."""
+    CAN = 0
+    CANFD = 1
+
+
+@dataclass
+class CanFrame:
+    """A single CAN frame."""
+    can_id: int
+    data: bytes
+    is_extended: bool = False
+    is_fd: bool = False
+    timestamp: float = 0.0
+
+    @property
+    def dlc(self) -> int:
+        return len(self.data)
+
+
+class CanTransport:
+    """SocketCAN transport — opens a CAN socket and provides send/recv.
+
+    Usage::
+
+        transport = CanTransport("can0")
+        transport.open()
+        transport.send(0x108, bytes([0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC]))
+        frame = transport.recv(timeout_s=0.1)
+        transport.close()
+    """
+
+    def __init__(self, channel: str, mode: CanMode = CanMode.CAN):
+        self._channel = channel
+        self._mode = mode
+        self._sock: Optional[socket.socket] = None
+        self._is_open = False
+        # Set of CAN IDs (SFF) we want the kernel to deliver. Empty = accept
+        # all (no hardware filter). Populated via set_id_filter() once the
+        # motor's mst_id is known, so that on a shared bus (e.g. a full arm
+        # chattering on other IDs) foreign frames don't flood the RX buffer
+        # and starve/drop the motor's status frames.
+        self._accept_ids: set = set()
+
+    # ── properties ──────────────────────────────────────────────────────
+
+    @property
+    def channel(self) -> str:
+        return self._channel
+
+    @property
+    def mode(self) -> CanMode:
+        return self._mode
+
+    @property
+    def is_open(self) -> bool:
+        return self._is_open
+
+    # ── open / close ────────────────────────────────────────────────────
+
+    def open(self) -> None:
+        """Open and bind the CAN socket.
+
+        Raises:
+            OSError: if the interface does not exist or is down.
+        """
+        if self._is_open:
+            return
+
+        sock = socket.socket(socket.PF_CAN, socket.SOCK_RAW, CAN_RAW)
+
+        # Check interface MTU.  Some USB-CAN adapters (gs_usb) and Linux
+        # kernel versions require CAN FD socket mode on FD-capable
+        # interfaces — classic CAN sockets may silently drop frames.
+        # Match the old damiao_socketcan behaviour: upgrade CAN→CANFD
+        # when the interface is FD, downgrade CANFD→CAN when not.
+        mtu = _iface_mtu(sock, self._channel)
+        if mtu is not None:
+            if mtu >= CANFD_MTU and self._mode == CanMode.CAN:
+                log.info(
+                    "Interface %s is CAN FD (MTU=%d); switching to CANFD mode.",
+                    self._channel, mtu,
+                )
+                self._mode = CanMode.CANFD
+            elif mtu < CANFD_MTU and self._mode == CanMode.CANFD:
+                log.warning(
+                    "Interface %s MTU is %d (classic CAN) but CanMode.CANFD "
+                    "requested; switching to classic CAN.",
+                    self._channel, mtu,
+                )
+                self._mode = CanMode.CAN
+
+        if self._mode == CanMode.CANFD:
+            try:
+                sock.setsockopt(SOL_CAN_RAW, CAN_RAW_FD_FRAMES, 1)
+            except OSError:
+                log.warning("Failed to enable CAN FD on %s; falling back to classic CAN.", self._channel)
+                self._mode = CanMode.CAN
+
+        try:
+            socket.if_nametoindex(self._channel)  # verify interface exists
+        except OSError as e:
+            sock.close()
+            raise OSError(f"CAN interface '{self._channel}' not found: {e}")
+
+        sock.bind((self._channel,))
+        sock.setblocking(False)
+
+        self._sock = sock
+        self._is_open = True
+        # Re-apply any ID filter requested before the socket existed.
+        if self._accept_ids:
+            self._apply_id_filter()
+        log.info("CAN transport opened on %s (mode=%s)", self._channel, self._mode.name)
+
+    # ── receive filtering ─────────────────────────────────────────────────
+
+    def set_id_filter(self, can_ids) -> None:
+        """Restrict which CAN IDs the kernel delivers to this socket.
+
+        On a shared bus (e.g. a full arm), other devices chatter on their own
+        IDs at high rate. Without a hardware filter every one of those frames
+        lands in this socket's RX buffer; a caller that reads one frame per
+        control cycle cannot keep up, the buffer saturates, and the motor's
+        own status frames are delayed or dropped — making read-back appear
+        "frozen" while the motor is actually moving.
+
+        Args:
+            can_ids: Iterable of SFF CAN IDs to accept. Empty/None clears the
+                     filter (accept all frames again).
+        """
+        self._accept_ids = {int(i) & CAN_SFF_MASK for i in (can_ids or [])}
+        if self._is_open:
+            self._apply_id_filter()
+
+    def add_id_filter(self, can_id: int) -> None:
+        """Add a single CAN ID to the accept set (see :meth:`set_id_filter`)."""
+        self.set_id_filter(self._accept_ids | {int(can_id) & CAN_SFF_MASK})
+
+    def _apply_id_filter(self) -> None:
+        """Push the current accept-id set to the socket via CAN_RAW_FILTER.
+
+        An empty set installs a zero-length filter list, which SocketCAN
+        interprets as "receive nothing" — so instead we remove the filter by
+        installing a single match-all entry (id=0, mask=0).
+        """
+        if self._sock is None:
+            return
+        if self._accept_ids:
+            fdata = b"".join(
+                struct.pack(_CAN_FILTER_FMT, cid, CAN_SFF_MASK)
+                for cid in sorted(self._accept_ids)
+            )
+        else:
+            fdata = struct.pack(_CAN_FILTER_FMT, 0, 0)  # match-all
+        try:
+            self._sock.setsockopt(SOL_CAN_RAW, CAN_RAW_FILTER, fdata)
+            log.info("CAN RX filter set on %s: %s", self._channel,
+                     [hex(i) for i in sorted(self._accept_ids)] or "all")
+        except OSError as e:
+            log.warning("Failed to set CAN RX filter on %s: %s", self._channel, e)
+
+    def close(self) -> None:
+        """Close the CAN socket."""
+        if not self._is_open or self._sock is None:
+            return
+        try:
+            self._sock.close()
+        except OSError:
+            pass
+        self._sock = None
+        self._is_open = False
+
+    # ── send ────────────────────────────────────────────────────────────
+
+    def send(self, can_id: int, data: bytes, timeout: float = 0.1) -> None:
+        """Send a CAN frame.
+
+        Args:
+            can_id: 11-bit or 29-bit CAN ID.
+            data: Payload (0–8 bytes classic, 0–64 bytes FD).
+            timeout: Max wait time if send buffer is full (seconds).
+
+        Raises:
+            TimeoutError: the send buffer stayed full for ``timeout`` seconds and
+                the frame could not be sent.  ``TimeoutError`` is an ``OSError``.
+                The frame is *not* dropped silently: a caller that cannot send
+                must find out, or it reports motion that never happened.
+        """
+        if not self._is_open or self._sock is None:
+            raise RuntimeError("Transport not open")
+
+        if self._mode == CanMode.CANFD:
+            flags = 0
+            frame = struct.pack(CANFD_FRAME_FMT,
+                                can_id & CAN_EFF_MASK,
+                                len(data), flags, 0, 0,
+                                data.ljust(64, b'\x00'))
+        else:
+            if len(data) > 8:
+                raise ValueError(f"Classic CAN frames max 8 bytes, got {len(data)}")
+            frame = struct.pack(CAN_FRAME_FMT,
+                                can_id & CAN_SFF_MASK,
+                                len(data),
+                                data.ljust(8, b'\x00'))
+
+        # Retry on BlockingIOError (send buffer full under high load).
+        deadline = time.monotonic() + timeout
+        while True:
+            try:
+                self._sock.send(frame)
+                return
+            except BlockingIOError:
+                remaining = deadline - time.monotonic()
+                if remaining <= 0:
+                    # 丢帧改报错：缓冲满意味着这一帧**没发出去**，而调用方拿到
+                    # 一个 None 就会当成发出去了（实测里正是这条链让夹爪
+                    # 「位移 0.000 mm 却返回 True」）。docstring 一直承诺的是
+                    # OSError，这里只是把实现改成与它一致。
+                    log.warning(
+                        "CAN send timeout on %s (buffer full >%.1fs), "
+                        "frame to 0x%03X not sent",
+                        self._channel, timeout, can_id & CAN_SFF_MASK,
+                    )
+                    raise TimeoutError(
+                        f"CAN 发送超时：{self._channel} 的发送缓冲在 {timeout:.1f} s "
+                        f"内一直满，帧 0x{can_id & CAN_SFF_MASK:03X} 没发出去。")
+                select.select([], [self._sock], [], remaining)
+
+    def send_multi(self, can_id: int, data: bytes, count: int,
+                   interval_s: float = 0.002) -> None:
+        """Send the same frame multiple times (for reliability of config commands)."""
+        for _ in range(count):
+            self.send(can_id, data)
+            if count > 1 and interval_s > 0:
+                time.sleep(interval_s)
+
+    # ── recv ────────────────────────────────────────────────────────────
+
+    def recv(self, timeout_s: float = 0.0) -> Optional[CanFrame]:
+        """Receive a single CAN frame.
+
+        Args:
+            timeout_s: Max wait time in seconds. 0 = non-blocking poll.
+
+        Returns:
+            CanFrame if a frame was received, None if timeout.
+        """
+        if not self._is_open or self._sock is None:
+            return None
+
+        try:
+            ready, _, _ = select.select([self._sock], [], [], timeout_s)
+            if not ready:
+                return None
+        except (OSError, ValueError):
+            return None
+
+        try:
+            raw = self._sock.recv(CANFD_FRAME_SIZE)
+        except OSError:
+            return None
+
+        ts = time.monotonic()
+
+        if self._mode == CanMode.CANFD:
+            can_id, dlc, flags, _len8, _res, data = struct.unpack(
+                CANFD_FRAME_FMT, raw)
+            is_fd = True
+            is_extended = bool(can_id & CAN_EFF_MASK)
+            data = data[:dlc]
+        else:
+            can_id, dlc, data = struct.unpack(CAN_FRAME_FMT, raw)
+            is_fd = False
+            is_extended = bool(can_id & CAN_EFF_MASK)
+            data = data[:dlc]
+
+        return CanFrame(
+            can_id=can_id & CAN_SFF_MASK,
+            data=data,
+            is_extended=is_extended,
+            is_fd=is_fd,
+            timestamp=ts,
+        )
+
+    def drain(self) -> None:
+        """Read and discard all pending frames from the receive buffer."""
+        while self.recv(timeout_s=0.0) is not None:
+            pass
+
+    # ── context manager ─────────────────────────────────────────────────
+
+    def __enter__(self) -> "CanTransport":
+        self.open()
+        return self
+
+    def __exit__(self, *args) -> None:
+        self.close()
+
+    def __repr__(self) -> str:
+        state = "open" if self._is_open else "closed"
+        return f"CanTransport({self._channel!r}, {self._mode.name}, {state})"

litegrip/constants.py

@@ -0,0 +1,146 @@
+"""LiteGrip SDK — constants and protocol definitions.
+
+Self-contained. Uses litegrip.can for DM motor types; does NOT import
+damiao_socketcan or any arm library.
+"""
+
+from typing import Final
+
+from .can.motor import MotorType
+from .can.protocol import ControlMode as _ControlMode
+
+
+# ═════════════════════════════════════════════════════════════════════════
+# Re-export DM protocol types for convenience
+# ═════════════════════════════════════════════════════════════════════════
+
+DM_Motor_Type = MotorType
+Control_Mode = _ControlMode
+
+
+# ═════════════════════════════════════════════════════════════════════════
+# LiteGrip gripper parameters
+# ═════════════════════════════════════════════════════════════════════════
+
+class GripperParams:
+    """LiteGrip default parameters."""
+
+    CAN_ID: Final = 0x08
+    MST_ID: Final = 0x18
+    MOTOR_TYPE: Final = MotorType.DM4310
+    CONTROL_MODE: Final = Control_Mode.MIT_MODE
+
+    # Position limits (rad) — nominal placeholders only.  The real limits come
+    # from the calibration; do not treat these as a direction convention, since
+    # a reverse-mounted motor has them the other way round.
+    POS_CLOSED_RAD: Final = 1.14
+    POS_OPEN_RAD: Final = 0.0
+
+    # MIT quantization limits (DM4310)
+    Q_MAX: Final = 12.5      # rad
+    DQ_MAX: Final = 30.0     # rad/s
+    TAU_MAX: Final = 10.0    # Nm
+
+    # Default control gains
+    DEFAULT_KP: Final = 100.0
+    DEFAULT_KD: Final = 2.0
+
+    # Fault recovery
+    FAULT_CLEAR_RETRIES: Final = 5
+    FAULT_CLEAR_DELAY_S: Final = 0.02
+
+
+# ═════════════════════════════════════════════════════════════════════════
+# Gripper geometry and unit conversion
+# ═════════════════════════════════════════════════════════════════════════
+
+class GripperGeometry:
+    """The LiteGrip two-finger gripper's nominal geometry.
+
+    These are the numbers every uncalibrated default in this package comes
+    from.  They are the measured geometry of the reference unit, not a
+    tolerance to be picked per gripper: an ``85 mm`` jaw travel is what the
+    calipers read, and the ``1 mm`` inset is how far the probe presses into
+    the open stop while looking for it.
+
+    **Keep the two apart; do not fold the inset into the travel.** The
+    calibration probe measures the span *between the two stops*, which is
+    ``SPAN_MM`` (86 mm) — the jaws at rest on the closed stop, and pressed
+    ~1 mm past where the jaws have run out of travel on the open one.  The
+    millimetres-per-radian scale is that span over the measured travel in
+    radians (``rad_to_mm``); deriving it from the 85 mm jaw travel instead
+    would leave every mm-based move 1.2% short — 1 mm lost per full stroke.
+    The factory calibration's own numbers show the pair: 1.170748 rad of
+    travel is 86 mm at ``73.45731105242119`` mm/rad, and 85 mm at
+    ``72.604``.
+
+    ``work_stroke_mm`` is measured from the closed zero like
+    :attr:`JAW_TRAVEL_MM`, so a full-stroke ``work_stroke_mm`` is 85 mm, not
+    ``SPAN_MM``.
+    """
+
+    #: Jaw travel measured with calipers, from the closed stop (mm).
+    JAW_TRAVEL_MM: Final = 85.0
+
+    #: How far the calibration probe presses into the open stop (mm).
+    STOP_INSET_MM: Final = 1.0
+
+    #: What the recorded extremes span (mm) — the numerator of the scale.
+    SPAN_MM: Final = JAW_TRAVEL_MM + STOP_INSET_MM
+
+
+class UnitConversion:
+    """Unit conversion coefficients.
+
+    Nominal values for this gripper — see :class:`GripperGeometry`.  They are
+    the span over the uncalibrated placeholder travel
+    (:attr:`~litegrip.models.GripperConfig.pos_closed_rad`, ``1.14 rad``), and
+    every motion is refused until a real calibration replaces them.  Run
+    ``calibrate()`` to get accurate per-unit values.
+    """
+    RAD_TO_MM: Final = GripperGeometry.SPAN_MM / 1.14   # ≈ 75.44 mm/rad
+    MM_TO_RAD: Final = 1.14 / GripperGeometry.SPAN_MM   # ≈ 0.01326 rad/mm
+    NM_TO_N: Final = 10.0             # approximate N per Nm
+    N_TO_NM: Final = 0.1              # approximate Nm per N
+
+
+# ═════════════════════════════════════════════════════════════════════════
+# Error codes
+# ═════════════════════════════════════════════════════════════════════════
+
+class ErrorCode:
+    """Damiao motor error codes (extracted from status frame data[0] >> 4)."""
+    DISABLED: Final = 0
+    ENABLED: Final = 1
+    UV_FAULT: Final = 0x9
+    OC_FAULT: Final = 0xA
+    MOS_OT: Final = 0xB
+    COIL_OT: Final = 0xC
+
+
+ERROR_DESCRIPTIONS = {
+    0x0: "已失能",
+    0x1: "已使能",
+    0x9: "欠压故障 (UV)",
+    0xA: "过流故障 (OC)",
+    0xB: "MOS 过温故障",
+    0xC: "线圈过温故障",
+}
+
+
+def describe_error(code: int) -> str:
+    """Return a human-readable description for a motor error code."""
+    return ERROR_DESCRIPTIONS.get(code, f"未知错误 (0x{code:X})")
+
+
+# ═════════════════════════════════════════════════════════════════════════
+# Default configuration
+# ═════════════════════════════════════════════════════════════════════════
+
+class DefaultParams:
+    """System defaults."""
+    CAN_CHANNEL: Final = "can0"
+    CAN_BITRATE: Final = 1_000_000    # 1 Mbps classic CAN
+    CANFD_MODE: Final = False
+    TIMEOUT_S: Final = 0.1
+    INIT_TIMEOUT_S: Final = 2.0

litegrip/exceptions.py

@@ -0,0 +1,47 @@
+"""LiteGrip SDK - 异常定义"""
+
+from __future__ import annotations
+
+
+class LiteGripError(Exception):
+    """LiteGrip 基础异常"""
+
+    def __init__(self, message: str, error_code: int | None = None):
+        super().__init__(message)
+        self.message = message
+        self.error_code = error_code
+
+    def __str__(self) -> str:
+        if self.error_code is not None:
+            return f"{self.message} [0x{self.error_code:04X}]"
+        return self.message
+
+
+class CommError(LiteGripError):
+    """通信错误（CAN 总线读写失败）"""
+    pass
+
+
+class ConnectError(LiteGripError):
+    """连接错误（CAN 接口不可用、电机无应答等）"""
+    pass
+
+
+class CommandError(LiteGripError):
+    """指令错误（电机拒绝执行或参数越界）"""
+    pass
+
+
+class CANTimeoutError(LiteGripError):
+    """超时错误（CAN 总线无应答）"""
+    pass
+
+
+class HardwareError(LiteGripError):
+    """硬件故障（欠压/过流/过温等达妙电机故障码）"""
+    pass
+
+
+class NotInitializedError(LiteGripError):
+    """未初始化错误（未连接/未使能时调用了需要使能的操作）"""
+    pass

litegrip/factory_calibration.json

@@ -0,0 +1,16 @@
+{
+  "channel": "can0",
+  "can_id": 8,
+  "canfd_mode": false,
+  "calibrated": true,
+  "zero_position_rad": 0.029564,
+  "max_position_rad": -1.141184,
+  "travel_range_rad": 1.170748,
+  "rad_to_mm": 73.45731105242119,
+  "work_stroke_mm": 80.0,
+  "motor_type": "DM4310",
+  "kp": 100.0,
+  "kd": 2.0,
+  "grasp_torque_threshold": 0.5,
+  "mst_id": 24
+}