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.
- r1_python_sdk/__init__.py +22 -0
- r1_python_sdk/api.py +40 -0
- r1_python_sdk/base.py +1075 -0
- r1_python_sdk/board.py +71 -0
- r1_python_sdk/code.py +161 -0
- r1_python_sdk/config.py +124 -0
- r1_python_sdk/damiao.py +239 -0
- r1_python_sdk/decorator.py +57 -0
- r1_python_sdk/gripper.py +125 -0
- r1_python_sdk/kin/__init__.py +19 -0
- r1_python_sdk/kin/chain.py +149 -0
- r1_python_sdk/kin/ik.py +180 -0
- r1_python_sdk/kin/transform.py +133 -0
- r1_python_sdk/kinematics.py +342 -0
- r1_python_sdk/logport.py +105 -0
- r1_python_sdk/protocol.py +354 -0
- r1_python_sdk/servo.py +287 -0
- r1_python_sdk/transport.py +246 -0
- r1_python_sdk/utils.py +50 -0
- r1_python_sdk/version.py +1 -0
- r1_python_sdk-0.2.0.dist-info/METADATA +399 -0
- r1_python_sdk-0.2.0.dist-info/RECORD +24 -0
- r1_python_sdk-0.2.0.dist-info/WHEEL +4 -0
- r1_python_sdk-0.2.0.dist-info/licenses/LICENSE +202 -0
|
@@ -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)
|
r1_python_sdk/version.py
ADDED
|
@@ -0,0 +1 @@
|
|
|
1
|
+
__version__ = "0.2.0"
|