r1-python-sdk 0.2.0__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
@@ -0,0 +1,246 @@
1
+ """Transport layer for the bottom board's USB-CAN bridge (Damiao USB-to-CAN serial protocol).
2
+
3
+ Host -> board: fixed 29 bytes
4
+ [0..1] 55 AA [2] 0x1E [3] 0x03 (send) / 0x02 (heartbeat)
5
+ [4..7] send count u32 [8..11] interval u32 (0.1 ms) [12] 0 standard / 1 extended frame
6
+ [13..16] ID u32 [17] 0 data / 1 remote frame [18] DLC [19] ID auto-increment [20] data auto-increment [21..28] data
7
+ Board -> host: fixed 16 bytes
8
+ [0] AA [1] command [2] bit0~5 DLC, bit6 IDE, bit7 RTR [3..6] ID [7..14] data [15] 55
9
+ commands: 0x00 heartbeat reply 0x11 CAN frame received 0x12 send OK
10
+ 0x02 send failed (bus error / host blocked / gripper motor dropped)
11
+ 0x20~0x2F board local reports: not CAN frames, the whole 13 bytes [2..14] are payload (protocol.decode_local_*)
12
+ Firmware implementation: R1BottomBoard/Core/Src/r1_usb_can.c; local report formats: r1_arm.h.
13
+ """
14
+
15
+ from __future__ import annotations
16
+
17
+ import threading
18
+ import time
19
+ from dataclasses import dataclass, field
20
+ from typing import Callable, List, Optional
21
+
22
+ import serial
23
+
24
+ SEND_LEN = 29
25
+ REPORT_LEN = 16
26
+
27
+ REP_HEARTBEAT = 0x00
28
+ REP_RX_FAIL = 0x01
29
+ REP_RX_OK = 0x11
30
+ REP_TX_FAIL = 0x02
31
+ REP_TX_OK = 0x12
32
+ # board local reports (firmware r1_usb_can.h R1_REP_LOCAL_*). In the callback frame.id = 0, frame.data = 13-byte payload
33
+ REP_LOCAL_STATUS = 0x20
34
+ REP_LOCAL_EVENT = 0x21
35
+ REP_LOCAL_POS = 0x22
36
+ REP_LOCAL_TAU = 0x23
37
+ REP_LOCAL_TAUG = 0x24
38
+ REP_LOCAL_VERSION = 0x25
39
+ LOCAL_PAYLOAD_LEN = 13
40
+
41
+
42
+ def is_local_report(cmd: int) -> bool:
43
+ return 0x20 <= cmd <= 0x2F
44
+
45
+
46
+ @dataclass
47
+ class CanFrame:
48
+ id: int
49
+ data: bytes
50
+ ide: bool = False
51
+ rtr: bool = False
52
+
53
+ @property
54
+ def dlc(self) -> int:
55
+ return len(self.data)
56
+
57
+
58
+ @dataclass
59
+ class BusStats:
60
+ tx_sent: int = 0
61
+ tx_ok: int = 0
62
+ tx_fail: int = 0
63
+ rx_ok: int = 0
64
+ rx_junk: int = 0
65
+ heartbeat: int = 0
66
+ local: int = 0
67
+ last_tx_fail_t: float = 0.0
68
+ last_tx_ok_t: float = 0.0
69
+ last_rx_t: float = 0.0
70
+ unknown_cmds: List[int] = field(default_factory=list)
71
+
72
+
73
+ ReportCallback = Callable[[int, Optional[CanFrame], float], None]
74
+
75
+
76
+ class DmUsbCan:
77
+ """Serial port + background receive thread. Callbacks run on the receive thread, so keep them fast."""
78
+
79
+ def __init__(self, port: str, baud: int = 921600) -> None:
80
+ self.port = port
81
+ self.baud = baud
82
+ self._ser: Optional[serial.Serial] = None
83
+ self._thread: Optional[threading.Thread] = None
84
+ self._stop = threading.Event()
85
+ self._callbacks: List[ReportCallback] = []
86
+ self._tx_lock = threading.Lock()
87
+ self.stats = BusStats()
88
+
89
+ # ---------------------------------------------------------------- lifecycle
90
+ @property
91
+ def is_open(self) -> bool:
92
+ return self._ser is not None and self._ser.is_open
93
+
94
+ def open(self) -> None:
95
+ if self.is_open:
96
+ return
97
+ self._ser = serial.Serial(self.port, self.baud, timeout=0.02, write_timeout=0.2)
98
+ self._ser.reset_input_buffer()
99
+ self._stop.clear()
100
+ self.stats = BusStats()
101
+ self._thread = threading.Thread(target=self._rx_loop, name="r1-usbcan-rx", daemon=True)
102
+ self._thread.start()
103
+
104
+ def close(self) -> None:
105
+ self._stop.set()
106
+ t = self._thread
107
+ if t is not None and t.is_alive() and t is not threading.current_thread():
108
+ t.join(1.0)
109
+ self._thread = None
110
+ ser = self._ser
111
+ self._ser = None
112
+ if ser is not None:
113
+ try:
114
+ ser.close()
115
+ except Exception:
116
+ pass
117
+
118
+ def add_callback(self, cb: ReportCallback) -> None:
119
+ self._callbacks.append(cb)
120
+
121
+ def remove_callback(self, cb: ReportCallback) -> None:
122
+ if cb in self._callbacks:
123
+ self._callbacks.remove(cb)
124
+
125
+ # ---------------------------------------------------------------- send
126
+ def send(self, frame: CanFrame, times: int = 1, gap_01ms: int = 10) -> None:
127
+ if not self.is_open:
128
+ raise serial.SerialException("port not open")
129
+ data = bytes(frame.data[:8])
130
+ b = bytearray(SEND_LEN)
131
+ b[0] = 0x55
132
+ b[1] = 0xAA
133
+ b[2] = 0x1E
134
+ b[3] = 0x03
135
+ b[4:8] = int(times).to_bytes(4, "little")
136
+ b[8:12] = int(gap_01ms).to_bytes(4, "little")
137
+ b[12] = 1 if frame.ide else 0
138
+ b[13:17] = int(frame.id).to_bytes(4, "little")
139
+ b[17] = 1 if frame.rtr else 0
140
+ b[18] = len(data)
141
+ b[21:21 + len(data)] = data
142
+ with self._tx_lock:
143
+ self._ser.write(bytes(b))
144
+ self.stats.tx_sent += 1
145
+
146
+ def send_std(self, can_id: int, data: bytes) -> None:
147
+ self.send(CanFrame(can_id, bytes(data)))
148
+
149
+ def send_many(self, frames: List[CanFrame]) -> None:
150
+ """Write several frames in one call to reduce USB packets (the board parses a byte stream, so concatenation is fine)."""
151
+ if not self.is_open:
152
+ raise serial.SerialException("port not open")
153
+ buf = bytearray()
154
+ for f in frames:
155
+ data = bytes(f.data[:8])
156
+ b = bytearray(SEND_LEN)
157
+ b[0] = 0x55
158
+ b[1] = 0xAA
159
+ b[2] = 0x1E
160
+ b[3] = 0x03
161
+ b[4:8] = (1).to_bytes(4, "little")
162
+ b[8:12] = (10).to_bytes(4, "little")
163
+ b[12] = 1 if f.ide else 0
164
+ b[13:17] = int(f.id).to_bytes(4, "little")
165
+ b[17] = 1 if f.rtr else 0
166
+ b[18] = len(data)
167
+ b[21:21 + len(data)] = data
168
+ buf += b
169
+ with self._tx_lock:
170
+ self._ser.write(bytes(buf))
171
+ self.stats.tx_sent += len(frames)
172
+
173
+ def heartbeat(self) -> None:
174
+ with self._tx_lock:
175
+ self._ser.write(bytes([0x55, 0x04, 0x00, 0xAA, 0x55]))
176
+
177
+ def stop_repeat(self) -> None:
178
+ with self._tx_lock:
179
+ self._ser.write(bytes([0x55, 0x03, 0x00, 0xAA, 0x55]))
180
+
181
+ # ---------------------------------------------------------------- receive
182
+ def _rx_loop(self) -> None:
183
+ buf = bytearray()
184
+ while not self._stop.is_set():
185
+ ser = self._ser
186
+ if ser is None:
187
+ break
188
+ try:
189
+ chunk = ser.read(ser.in_waiting or 1)
190
+ except Exception:
191
+ self._emit(-1, None, time.monotonic()) # -1: serial port died
192
+ break
193
+ if not chunk:
194
+ continue
195
+ buf += chunk
196
+ now = time.monotonic()
197
+ i = 0
198
+ n = len(buf)
199
+ while n - i >= REPORT_LEN:
200
+ if buf[i] != 0xAA or buf[i + REPORT_LEN - 1] != 0x55:
201
+ i += 1
202
+ self.stats.rx_junk += 1
203
+ continue
204
+ self._handle(buf[i:i + REPORT_LEN], now)
205
+ i += REPORT_LEN
206
+ del buf[:i]
207
+ if len(buf) > 4096:
208
+ del buf[:-REPORT_LEN]
209
+
210
+ def _handle(self, r: bytes, now: float) -> None:
211
+ cmd = r[1]
212
+ frame: Optional[CanFrame] = None
213
+ if is_local_report(cmd):
214
+ frame = CanFrame(id=0, data=bytes(r[2:2 + LOCAL_PAYLOAD_LEN]))
215
+ elif cmd != REP_HEARTBEAT:
216
+ dlc = r[2] & 0x3F
217
+ frame = CanFrame(
218
+ id=int.from_bytes(r[3:7], "little"),
219
+ data=bytes(r[7:7 + min(dlc, 8)]),
220
+ ide=bool(r[2] & 0x40),
221
+ rtr=bool(r[2] & 0x80),
222
+ )
223
+ st = self.stats
224
+ if cmd == REP_RX_OK:
225
+ st.rx_ok += 1
226
+ st.last_rx_t = now
227
+ elif cmd == REP_TX_OK:
228
+ st.tx_ok += 1
229
+ st.last_tx_ok_t = now
230
+ elif cmd == REP_TX_FAIL:
231
+ st.tx_fail += 1
232
+ st.last_tx_fail_t = now
233
+ elif cmd == REP_HEARTBEAT:
234
+ st.heartbeat += 1
235
+ elif is_local_report(cmd):
236
+ st.local += 1
237
+ elif cmd not in st.unknown_cmds:
238
+ st.unknown_cmds.append(cmd)
239
+ self._emit(cmd, frame, now)
240
+
241
+ def _emit(self, cmd: int, frame: Optional[CanFrame], now: float) -> None:
242
+ for cb in list(self._callbacks):
243
+ try:
244
+ cb(cmd, frame, now)
245
+ except Exception:
246
+ pass
r1_python_sdk/utils.py ADDED
@@ -0,0 +1,50 @@
1
+ from __future__ import annotations
2
+
3
+ import math
4
+ from typing import Iterable, List, Optional, Sequence
5
+
6
+ from serial.tools import list_ports
7
+
8
+ from .protocol import BOARD_USB_VID, BOTTOM_CDC_PID
9
+
10
+
11
+ def find_bottom_board_ports() -> List[str]:
12
+ """CDC ports of the bottom board's USB-CAN bridge (VID 0483 / PID 7154)."""
13
+ out = []
14
+ for p in list_ports.comports():
15
+ if p.vid == BOARD_USB_VID and p.pid == BOTTOM_CDC_PID:
16
+ out.append(p.device)
17
+ else:
18
+ blob = " ".join(x for x in (p.description, p.hwid, p.product) if x).upper()
19
+ if "RR R1 BOTTOM BOARD" in blob or ("VID_0483" in blob and "PID_7154" in blob):
20
+ out.append(p.device)
21
+ return sorted(set(out))
22
+
23
+
24
+ def find_one_bottom_board() -> Optional[str]:
25
+ ports = find_bottom_board_ports()
26
+ return ports[0] if len(ports) == 1 else None
27
+
28
+
29
+ def to_rad(v: float, is_radian: bool) -> float:
30
+ return float(v) if is_radian else math.radians(float(v))
31
+
32
+
33
+ def from_rad(v: float, is_radian: bool) -> float:
34
+ return float(v) if is_radian else math.degrees(float(v))
35
+
36
+
37
+ def seq_to_rad(vals: Iterable[float], is_radian: bool) -> List[float]:
38
+ return [to_rad(v, is_radian) for v in vals]
39
+
40
+
41
+ def seq_from_rad(vals: Iterable[float], is_radian: bool) -> List[float]:
42
+ return [from_rad(v, is_radian) for v in vals]
43
+
44
+
45
+ def is_number_seq(x, n: Optional[int] = None) -> bool:
46
+ if not isinstance(x, Sequence) or isinstance(x, (str, bytes)):
47
+ return False
48
+ if n is not None and len(x) != n:
49
+ return False
50
+ return all(isinstance(v, (int, float)) and not isinstance(v, bool) for v in x)
@@ -0,0 +1 @@
1
+ __version__ = "0.2.0"