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,354 @@
|
|
|
1
|
+
"""Constants of the R1 bottom-board / end-board custom protocol, and parsing of the two sources of board status:
|
|
2
|
+
local reports on the USB-CAN bridge (``decode_local_*``, the main path) and ``$S`` / ``$E`` lines on the USART2
|
|
3
|
+
log port (``parse_log_line``, for debugging). Both yield the same BoardStatus / BoardEvent.
|
|
4
|
+
|
|
5
|
+
Constants mirror the firmware:
|
|
6
|
+
- R1BottomBoard/Core/Inc/r1_arm.h state machine, flags, event codes, $S/$E and local report formats
|
|
7
|
+
- R1BottomBoard/Core/Inc/r1_usb_can.h local report command numbers R1_REP_LOCAL_*
|
|
8
|
+
- R1EndBoard/Core/Inc/r1_grip_can.h gripper 0x7F1 / 0x7F2
|
|
9
|
+
- R1EndBoard/Core/Inc/r1_btn_can.h end-effector buttons 0x7F0
|
|
10
|
+
- r1_ui_can.h UI cues 0x7F3
|
|
11
|
+
- R1BottomBoard/Core/Inc/r1_arm.h PC -> board local commands 0x7F4 (R1_HOST_CMD_*)
|
|
12
|
+
Changing the firmware means changing this file too. Angles on the log port are 0.01 deg (cdeg); in local reports
|
|
13
|
+
they are the raw Damiao 16-bit encoding.
|
|
14
|
+
"""
|
|
15
|
+
|
|
16
|
+
from __future__ import annotations
|
|
17
|
+
|
|
18
|
+
import math
|
|
19
|
+
from dataclasses import dataclass, field
|
|
20
|
+
from typing import List, Optional, Sequence, Tuple, Union
|
|
21
|
+
|
|
22
|
+
NJ = 6
|
|
23
|
+
JOINT_ESC_BASE = 0x01 # joint1 = CAN ID 0x01
|
|
24
|
+
JOINT_MST_BASE = 0x10 # feedback ID = 0x10 + motor number
|
|
25
|
+
GRIPPER_ESC_ID = 0x07 # gripper motor, owned by the end board; frames sent directly by the PC are dropped by the bottom board
|
|
26
|
+
|
|
27
|
+
# USB
|
|
28
|
+
BOARD_USB_VID = 0x0483
|
|
29
|
+
BOTTOM_CDC_PID = 0x7154 # RR R1 BOTTOM BOARD (USB-CAN bridge)
|
|
30
|
+
END_CDC_PID = 0x7152
|
|
31
|
+
IAP_CDC_PID = 0x7153
|
|
32
|
+
BOTTOM_CDC_BAUD = 921600 # CDC ignores the baud rate; kept as the customary Damiao USB-CAN value
|
|
33
|
+
LOG_BAUD = 115200 # USART2 log port
|
|
34
|
+
|
|
35
|
+
STAT_PERIOD_MS = 40
|
|
36
|
+
REC_PERIOD_MS = 40
|
|
37
|
+
|
|
38
|
+
# ---- custom CAN IDs ----
|
|
39
|
+
CAN_ID_BTN = 0x7F0 # end -> bottom: button heartbeat
|
|
40
|
+
CAN_ID_GRIP_CMD = 0x7F1 # bottom/PC -> end: gripper command
|
|
41
|
+
CAN_ID_GRIP_ST = 0x7F2 # end -> bottom: gripper status
|
|
42
|
+
CAN_ID_UI = 0x7F3 # bottom -> end: LED / buzzer cues
|
|
43
|
+
CAN_ID_HOST_CMD = 0x7F4 # PC -> bottom: local command (intercepted by the board, never on the bus; accepted even while blocked; r1_arm.h R1_HOST_CMD_*)
|
|
44
|
+
HOST_CMD_PAUSE = 0x01 # data[0]: soft E-stop -- during teach / replay make the board enter HOLD and pin. Reply TX_OK = pinned
|
|
45
|
+
HOST_CMD_GET_VERSION = 0x02 # data[0]: query version -- board sends local report 0x25 VERSION then replies TX_OK; old firmware replies TX_FAIL
|
|
46
|
+
|
|
47
|
+
GRIP_MAGIC = 0x47
|
|
48
|
+
GRIP_CMD_QUERY = 0x00
|
|
49
|
+
GRIP_CMD_HOME = 0x01
|
|
50
|
+
GRIP_CMD_STOP = 0x02
|
|
51
|
+
GRIP_CMD_POS = 0x03
|
|
52
|
+
GRIP_CMD_HOLD = 0x04
|
|
53
|
+
GRIP_CMD_PARAM = 0x05 # speed / acc / dec / current limit, see grip_param_frame
|
|
54
|
+
|
|
55
|
+
# 0x7F1 PARAM data[2] bit mask: only fields with their bit set are changed
|
|
56
|
+
GRIP_PMASK_SPEED = 0x01
|
|
57
|
+
GRIP_PMASK_ACC = 0x02
|
|
58
|
+
GRIP_PMASK_DEC = 0x04
|
|
59
|
+
GRIP_PMASK_CURRENT = 0x08
|
|
60
|
+
|
|
61
|
+
GRIP_ST_OFF = 0
|
|
62
|
+
GRIP_ST_HOMING = 1
|
|
63
|
+
GRIP_ST_READY = 2
|
|
64
|
+
GRIP_ST_FAULT = 3
|
|
65
|
+
GRIP_ST_NAMES = {0: "not homed", 1: "homing", 2: "ready", 3: "fault"}
|
|
66
|
+
GRIP_ST_HB_MS = 100
|
|
67
|
+
|
|
68
|
+
|
|
69
|
+
def grip_cmd_frame(cmd: int, pos_percent: int = 0) -> bytes:
|
|
70
|
+
p = max(0, min(100, int(pos_percent)))
|
|
71
|
+
return bytes([GRIP_MAGIC, cmd & 0xFF, p, 0, 0, 0, 0, 0])
|
|
72
|
+
|
|
73
|
+
|
|
74
|
+
def grip_param_frame(speed: Optional[int] = None, acc: Optional[int] = None,
|
|
75
|
+
dec: Optional[int] = None, current: Optional[int] = None,
|
|
76
|
+
save: bool = False) -> bytes:
|
|
77
|
+
"""0x7F1 PARAM: fields that are None are left unchanged. Values 0~255 (AgiBot OmniPicker register scale).
|
|
78
|
+
[2] mask [3] speed [4] acc [5] dec [6] current [7] save"""
|
|
79
|
+
mask = 0
|
|
80
|
+
vals = []
|
|
81
|
+
for bit, v in ((GRIP_PMASK_SPEED, speed), (GRIP_PMASK_ACC, acc),
|
|
82
|
+
(GRIP_PMASK_DEC, dec), (GRIP_PMASK_CURRENT, current)):
|
|
83
|
+
if v is None:
|
|
84
|
+
vals.append(0)
|
|
85
|
+
else:
|
|
86
|
+
mask |= bit
|
|
87
|
+
vals.append(max(0, min(255, int(v))))
|
|
88
|
+
return bytes([GRIP_MAGIC, GRIP_CMD_PARAM, mask, *vals, 1 if save else 0])
|
|
89
|
+
|
|
90
|
+
|
|
91
|
+
@dataclass
|
|
92
|
+
class GripperStatus:
|
|
93
|
+
status: int # GRIP_ST_*
|
|
94
|
+
homed: bool
|
|
95
|
+
reason: int
|
|
96
|
+
seq: int
|
|
97
|
+
percent: int # current opening 0~100
|
|
98
|
+
angle_deg: float # angle relative to the zero point
|
|
99
|
+
|
|
100
|
+
@property
|
|
101
|
+
def status_name(self) -> str:
|
|
102
|
+
return GRIP_ST_NAMES.get(self.status, f"status {self.status}")
|
|
103
|
+
|
|
104
|
+
|
|
105
|
+
def decode_grip_status(data: bytes) -> Optional[GripperStatus]:
|
|
106
|
+
if len(data) < 8 or data[0] != GRIP_MAGIC:
|
|
107
|
+
return None
|
|
108
|
+
ang = int.from_bytes(data[6:8], "little", signed=True) / 10.0
|
|
109
|
+
return GripperStatus(status=data[1], homed=bool(data[2]), reason=data[3],
|
|
110
|
+
seq=data[4], percent=data[5], angle_deg=ang)
|
|
111
|
+
|
|
112
|
+
|
|
113
|
+
# ---- board state machine ----
|
|
114
|
+
ST_BOOT, ST_NORMAL, ST_PROBE, ST_BRINGUP, ST_TEACH, ST_HOLD, ST_REPLAY, ST_ESTOP = range(8)
|
|
115
|
+
STATE_SHORT = ["BOOT", "NORMAL", "PROBE", "BRINGUP", "TEACH", "HOLD", "REPLAY", "ESTOP"]
|
|
116
|
+
|
|
117
|
+
SF_ESTOP = 1 << 0
|
|
118
|
+
SF_BLUE = 1 << 1
|
|
119
|
+
SF_GREEN = 1 << 2
|
|
120
|
+
SF_ALARM = 1 << 3
|
|
121
|
+
SF_HOST_BLOCKED = 1 << 4
|
|
122
|
+
SF_PLAY_FWD = 1 << 5
|
|
123
|
+
SF_BTN_ALIVE = 1 << 6
|
|
124
|
+
SF_ESTOP_OURS = 1 << 7
|
|
125
|
+
|
|
126
|
+
|
|
127
|
+
@dataclass
|
|
128
|
+
class BoardStatus:
|
|
129
|
+
"""Board status snapshot: one ``$S`` line, or USB local report 0x20 STATUS (positions / torques filled in from
|
|
130
|
+
0x22/0x23/0x24)."""
|
|
131
|
+
state: int
|
|
132
|
+
flags: int
|
|
133
|
+
fb_mask: int
|
|
134
|
+
traj_n: int
|
|
135
|
+
progress: int
|
|
136
|
+
modes: List[int] # 6 entries, 0xF = unknown
|
|
137
|
+
dm_status: List[int] # 6 entries, Damiao status nibble
|
|
138
|
+
ev_seq: int
|
|
139
|
+
pos: List[float] = field(default_factory=list) # rad; empty for local reports until 0x22 arrives
|
|
140
|
+
tau: List[float] = field(default_factory=list) # Nm
|
|
141
|
+
tau_g: List[float] = field(default_factory=list) # Nm, gravity feed-forward
|
|
142
|
+
junk: int = 0 # only carried by $S
|
|
143
|
+
t: float = 0.0 # receive time, time.monotonic()
|
|
144
|
+
|
|
145
|
+
def flag(self, bit: int) -> bool:
|
|
146
|
+
return bool(self.flags & bit)
|
|
147
|
+
|
|
148
|
+
def fb_ok(self, j: int) -> bool:
|
|
149
|
+
return bool(self.fb_mask & (1 << j))
|
|
150
|
+
|
|
151
|
+
@property
|
|
152
|
+
def state_name(self) -> str:
|
|
153
|
+
return STATE_SHORT[self.state] if 0 <= self.state < len(STATE_SHORT) else f"?{self.state}"
|
|
154
|
+
|
|
155
|
+
|
|
156
|
+
# ---- events ----
|
|
157
|
+
EV_NONE = 0
|
|
158
|
+
EV_BOOT = 1
|
|
159
|
+
EV_STATE = 2
|
|
160
|
+
EV_HOT_HANDOVER = 3
|
|
161
|
+
EV_COLD_PROBE = 4
|
|
162
|
+
EV_ENABLING = 5
|
|
163
|
+
EV_TRAJ_SAVED = 6
|
|
164
|
+
EV_REC_SHORT = 7
|
|
165
|
+
EV_REC_NOMOVE = 8
|
|
166
|
+
EV_REC_FULL = 9
|
|
167
|
+
EV_REPLAY_START = 10
|
|
168
|
+
EV_REPLAY_DONE = 11
|
|
169
|
+
EV_REPLAY_PAUSE = 12
|
|
170
|
+
EV_REPLAY_ABORT = 13
|
|
171
|
+
EV_NO_TRAJ = 14
|
|
172
|
+
EV_FB_LOST = 15
|
|
173
|
+
EV_FB_OK = 16
|
|
174
|
+
EV_MODE_BAD = 17
|
|
175
|
+
EV_ESTOP_ON = 18
|
|
176
|
+
EV_ESTOP_OFF = 19
|
|
177
|
+
EV_HOST_TAKEOVER = 20
|
|
178
|
+
EV_BTN_LOST = 21
|
|
179
|
+
EV_RETRY = 22
|
|
180
|
+
EV_ESTOP_TAKEOVER = 23
|
|
181
|
+
EV_HOST_PAUSE = 24
|
|
182
|
+
|
|
183
|
+
EV_NAMES = {
|
|
184
|
+
1: "BOOT", 2: "STATE", 3: "HOT_HANDOVER", 4: "COLD_PROBE", 5: "ENABLING", 6: "TRAJ_SAVED",
|
|
185
|
+
7: "REC_SHORT", 8: "REC_NOMOVE", 9: "REC_FULL", 10: "REPLAY_START", 11: "REPLAY_DONE",
|
|
186
|
+
12: "REPLAY_PAUSE", 13: "REPLAY_ABORT", 14: "NO_TRAJ", 15: "FB_LOST", 16: "FB_OK",
|
|
187
|
+
17: "MODE_BAD", 18: "ESTOP_ON", 19: "ESTOP_OFF", 20: "HOST_TAKEOVER", 21: "BTN_LOST", 22: "RETRY",
|
|
188
|
+
23: "ESTOP_TAKEOVER", 24: "HOST_PAUSE",
|
|
189
|
+
}
|
|
190
|
+
|
|
191
|
+
|
|
192
|
+
@dataclass
|
|
193
|
+
class BoardEvent:
|
|
194
|
+
"""Board event: one ``$E`` line, or USB local report 0x21 EVENT."""
|
|
195
|
+
seq: int
|
|
196
|
+
code: int
|
|
197
|
+
arg: int # i16
|
|
198
|
+
t: float = 0.0
|
|
199
|
+
|
|
200
|
+
@property
|
|
201
|
+
def name(self) -> str:
|
|
202
|
+
return EV_NAMES.get(self.code, f"EV{self.code}")
|
|
203
|
+
|
|
204
|
+
def text(self) -> str:
|
|
205
|
+
a, c = self.arg, self.code
|
|
206
|
+
if c == EV_STATE:
|
|
207
|
+
return f"state {_st(a & 0xFF)} → {_st((a >> 8) & 0xFF)}"
|
|
208
|
+
if c == EV_ESTOP_ON:
|
|
209
|
+
return "E-stop pressed: " + ("board already pinning in MIT" if a
|
|
210
|
+
else "PC was in control, board switches each axis to MIT and takes over")
|
|
211
|
+
if c == EV_ESTOP_TAKEOVER:
|
|
212
|
+
return f"E-stop takeover done: pinned 0x{a & 0xFF:02X}, gave up 0x{(a >> 8) & 0xFF:02X}"
|
|
213
|
+
if c == EV_ESTOP_OFF:
|
|
214
|
+
return "E-stop released (board in HOLD, the PC must enable again to take over)"
|
|
215
|
+
if c == EV_HOST_TAKEOVER:
|
|
216
|
+
return f"PC takeover (frame ID 0x{a & 0xFFFF:03X})"
|
|
217
|
+
if c == EV_HOST_PAUSE:
|
|
218
|
+
return f"PC soft E-stop: {_st(a & 0xFF)} → HOLD, pinned"
|
|
219
|
+
if c == EV_FB_LOST:
|
|
220
|
+
return f"joint{a} feedback lost"
|
|
221
|
+
if c == EV_FB_OK:
|
|
222
|
+
return f"joint{a} feedback restored"
|
|
223
|
+
if c == EV_REPLAY_DONE:
|
|
224
|
+
return f"replay done, max tracking error {a / 100:.2f}°"
|
|
225
|
+
if c == EV_REPLAY_ABORT:
|
|
226
|
+
return f"replay aborted: joint{a & 0x0F} tracking error {((a >> 4) & 0xFFF) / 100:.2f}°"
|
|
227
|
+
if c == EV_TRAJ_SAVED:
|
|
228
|
+
return f"trajectory saved, {a} points"
|
|
229
|
+
if c == EV_MODE_BAD:
|
|
230
|
+
return f"joint{a & 0xFF} mode {(a >> 8) & 0xFF} is not MIT"
|
|
231
|
+
return f"{self.name} arg={a}"
|
|
232
|
+
|
|
233
|
+
|
|
234
|
+
def _st(code: int) -> str:
|
|
235
|
+
return STATE_SHORT[code] if 0 <= code < len(STATE_SHORT) else f"?{code}"
|
|
236
|
+
|
|
237
|
+
|
|
238
|
+
@dataclass
|
|
239
|
+
class LogLine:
|
|
240
|
+
text: str
|
|
241
|
+
|
|
242
|
+
|
|
243
|
+
Parsed = Union[BoardStatus, BoardEvent, LogLine]
|
|
244
|
+
|
|
245
|
+
|
|
246
|
+
def _hex_nibbles(s: str, n: int) -> List[int]:
|
|
247
|
+
out = [int(ch, 16) for ch in s[:n]]
|
|
248
|
+
while len(out) < n:
|
|
249
|
+
out.append(0xF)
|
|
250
|
+
return out
|
|
251
|
+
|
|
252
|
+
|
|
253
|
+
def parse_log_line(line: str, t: float = 0.0) -> Optional[Parsed]:
|
|
254
|
+
"""One line (without newline) -> BoardStatus / BoardEvent / LogLine; lines starting with '$' but malformed return None."""
|
|
255
|
+
line = line.rstrip("\r\n")
|
|
256
|
+
if not line:
|
|
257
|
+
return None
|
|
258
|
+
if line[0] != "$":
|
|
259
|
+
return LogLine(line)
|
|
260
|
+
f = line.split(",")
|
|
261
|
+
try:
|
|
262
|
+
if f[0] == "$S" and len(f) >= 10 + 3 * NJ:
|
|
263
|
+
return BoardStatus(
|
|
264
|
+
state=int(f[1]),
|
|
265
|
+
flags=int(f[2], 16),
|
|
266
|
+
fb_mask=int(f[3], 16),
|
|
267
|
+
traj_n=int(f[4]),
|
|
268
|
+
progress=int(f[5]),
|
|
269
|
+
modes=_hex_nibbles(f[6], NJ),
|
|
270
|
+
dm_status=_hex_nibbles(f[7], NJ),
|
|
271
|
+
ev_seq=int(f[8]),
|
|
272
|
+
pos=[math.radians(int(x) / 100.0) for x in f[9:9 + NJ]],
|
|
273
|
+
tau=[int(x) / 100.0 for x in f[9 + NJ:9 + 2 * NJ]],
|
|
274
|
+
tau_g=[int(x) / 100.0 for x in f[9 + 2 * NJ:9 + 3 * NJ]],
|
|
275
|
+
junk=int(f[9 + 3 * NJ]),
|
|
276
|
+
t=t,
|
|
277
|
+
)
|
|
278
|
+
if f[0] == "$E" and len(f) >= 4:
|
|
279
|
+
return BoardEvent(int(f[1]) & 0xFF, int(f[2]), int(f[3]), t=t)
|
|
280
|
+
except ValueError:
|
|
281
|
+
return None
|
|
282
|
+
return None
|
|
283
|
+
|
|
284
|
+
|
|
285
|
+
# ---- local reports on the USB-CAN bridge (firmware r1_arm.h "local report", r1_usb_can.h R1_REP_LOCAL_*) ----
|
|
286
|
+
LOCAL_PAYLOAD_LEN = 13
|
|
287
|
+
|
|
288
|
+
|
|
289
|
+
def _nibbles6(p: bytes, off: int) -> List[int]:
|
|
290
|
+
"""Six nibbles in [off..off+2]: joint1 = low nibble, joint2 = high nibble, ..."""
|
|
291
|
+
out = []
|
|
292
|
+
for j in range(NJ):
|
|
293
|
+
b = p[off + (j >> 1)]
|
|
294
|
+
out.append((b >> 4) & 0x0F if (j & 1) else b & 0x0F)
|
|
295
|
+
return out
|
|
296
|
+
|
|
297
|
+
|
|
298
|
+
def decode_local_status(p: bytes, t: float = 0.0, prev: Optional[BoardStatus] = None) -> Optional[BoardStatus]:
|
|
299
|
+
"""0x20 STATUS -> BoardStatus. Positions / torques are carried over from ``prev`` (they are refreshed separately
|
|
300
|
+
by 0x22/0x23/0x24)."""
|
|
301
|
+
if len(p) < LOCAL_PAYLOAD_LEN:
|
|
302
|
+
return None
|
|
303
|
+
traj_n = p[4] | ((p[6] & 0x0F) << 8)
|
|
304
|
+
prog = p[5] | ((p[6] >> 4) << 8)
|
|
305
|
+
return BoardStatus(
|
|
306
|
+
state=p[0], flags=p[1], fb_mask=p[2], traj_n=traj_n, progress=prog,
|
|
307
|
+
modes=_nibbles6(p, 7), dm_status=_nibbles6(p, 10), ev_seq=p[3],
|
|
308
|
+
pos=list(prev.pos) if prev else [], tau=list(prev.tau) if prev else [],
|
|
309
|
+
tau_g=list(prev.tau_g) if prev else [], junk=prev.junk if prev else 0, t=t,
|
|
310
|
+
)
|
|
311
|
+
|
|
312
|
+
|
|
313
|
+
def decode_local_event(p: bytes, t: float = 0.0) -> Optional[BoardEvent]:
|
|
314
|
+
"""0x21 EVENT → BoardEvent."""
|
|
315
|
+
if len(p) < 4:
|
|
316
|
+
return None
|
|
317
|
+
return BoardEvent(p[0], p[1], int.from_bytes(p[2:4], "little", signed=True), t=t)
|
|
318
|
+
|
|
319
|
+
|
|
320
|
+
def decode_local_pos(p: bytes, pmax: Sequence[float]) -> List[float]:
|
|
321
|
+
"""0x22 POS: 6 x u16 raw Damiao encoding -> rad. ``pmax`` is the per-joint PMAX (damiao.R1_JOINT_LIMITS[j].pmax)."""
|
|
322
|
+
out = []
|
|
323
|
+
for j in range(NJ):
|
|
324
|
+
u = int.from_bytes(p[2 * j:2 * j + 2], "little")
|
|
325
|
+
out.append(u * (2.0 * pmax[j]) / 65535.0 - pmax[j])
|
|
326
|
+
return out
|
|
327
|
+
|
|
328
|
+
|
|
329
|
+
def decode_local_i16x6(p: bytes, scale: float = 0.01) -> List[float]:
|
|
330
|
+
"""0x23 TAU / 0x24 TAUG: 6 × i16 × scale."""
|
|
331
|
+
return [int.from_bytes(p[2 * j:2 * j + 2], "little", signed=True) * scale for j in range(NJ)]
|
|
332
|
+
|
|
333
|
+
|
|
334
|
+
@dataclass
|
|
335
|
+
class BoardVersion:
|
|
336
|
+
"""Bottom-board firmware version: USB local report 0x25 VERSION (one on power-up, another on ``HOST_CMD_GET_VERSION``)."""
|
|
337
|
+
board: int # r1_board_id.h: 1 end board, 2 bottom board
|
|
338
|
+
version: Tuple[int, int, int] # firmware major/minor/patch (r1_version.h)
|
|
339
|
+
min_tool: Tuple[int, int, int] # minimum R1-Board-Config version required by the firmware
|
|
340
|
+
|
|
341
|
+
@property
|
|
342
|
+
def text(self) -> str:
|
|
343
|
+
return "%d.%d.%d" % self.version
|
|
344
|
+
|
|
345
|
+
@property
|
|
346
|
+
def min_tool_text(self) -> str:
|
|
347
|
+
return "%d.%d.%d" % self.min_tool
|
|
348
|
+
|
|
349
|
+
|
|
350
|
+
def decode_local_version(p: bytes) -> Optional[BoardVersion]:
|
|
351
|
+
"""0x25 VERSION -> BoardVersion. [0] board type [1..3] firmware version [4..6] minimum tool version [7..12] reserved."""
|
|
352
|
+
if len(p) < 7:
|
|
353
|
+
return None
|
|
354
|
+
return BoardVersion(p[0], (p[1], p[2], p[3]), (p[4], p[5], p[6]))
|
r1_python_sdk/servo.py
ADDED
|
@@ -0,0 +1,287 @@
|
|
|
1
|
+
"""Servo: joint-space APIs."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
import math
|
|
6
|
+
import time
|
|
7
|
+
from typing import List, Optional, Sequence, Tuple
|
|
8
|
+
|
|
9
|
+
from . import damiao as dm
|
|
10
|
+
from .code import ArmMode, R1State
|
|
11
|
+
from .decorator import r1_catch, r1_connected, r1_ready
|
|
12
|
+
from .utils import from_rad, is_number_seq, seq_from_rad, to_rad
|
|
13
|
+
from .base import NJ, POS_SANE_RAD, Base
|
|
14
|
+
|
|
15
|
+
REACH_TOL_RAD = math.radians(0.5)
|
|
16
|
+
REACH_VEL_RAD = 0.05
|
|
17
|
+
ZERO_TOL_RAD = math.radians(1.0)
|
|
18
|
+
|
|
19
|
+
|
|
20
|
+
class Servo(Base):
|
|
21
|
+
|
|
22
|
+
# ------------------------------------------------------------------ motion
|
|
23
|
+
@r1_catch("set")
|
|
24
|
+
@r1_ready("set")
|
|
25
|
+
def set_servo_angle(self, angle=None, speed: Optional[float] = None, wait: bool = False,
|
|
26
|
+
timeout: Optional[float] = None, servo_id: Optional[int] = None,
|
|
27
|
+
relative: bool = False, is_radian: Optional[bool] = None) -> int:
|
|
28
|
+
"""Move to a target in position mode (set_mode(0)).
|
|
29
|
+
With servo_id=None, angle is 6 values; otherwise servo_id is 1~6 and angle is a single value.
|
|
30
|
+
speed is the speed cap during the move (deg/s or rad/s, Damiao v_des); defaults to config.speed_default.
|
|
31
|
+
wait=True blocks until all six joints are within +/-0.5 deg and at rest, or returns WAIT_FINISH_TIMEOUT."""
|
|
32
|
+
if self._mode != ArmMode.POSITION:
|
|
33
|
+
self._set_last_error("set_servo_angle: needs set_mode(0)")
|
|
34
|
+
return R1State.MODE_MISMATCH
|
|
35
|
+
rad = self._is_radian if is_radian is None else bool(is_radian)
|
|
36
|
+
if servo_id is None:
|
|
37
|
+
if not is_number_seq(angle, NJ):
|
|
38
|
+
return R1State.PARAM_ERROR
|
|
39
|
+
tgt = [to_rad(a, rad) for a in angle]
|
|
40
|
+
idx = list(range(NJ))
|
|
41
|
+
else:
|
|
42
|
+
idx1 = self._joint_indices(servo_id)
|
|
43
|
+
if idx1 is None:
|
|
44
|
+
return R1State.SERVO_NOT_EXIST
|
|
45
|
+
if not isinstance(angle, (int, float)) or isinstance(angle, bool):
|
|
46
|
+
return R1State.PARAM_ERROR
|
|
47
|
+
idx = idx1
|
|
48
|
+
tgt = [to_rad(angle, rad)]
|
|
49
|
+
with self._lock:
|
|
50
|
+
if relative:
|
|
51
|
+
tgt = [self._joints[j].target + t for j, t in zip(idx, tgt)]
|
|
52
|
+
code = self._check_targets(idx, tgt, rad)
|
|
53
|
+
if code != R1State.NORMAL:
|
|
54
|
+
return code
|
|
55
|
+
vlim = self._speed_rad(speed, rad)
|
|
56
|
+
if vlim is None:
|
|
57
|
+
return R1State.OUT_OF_RANGE
|
|
58
|
+
for j, t in zip(idx, tgt):
|
|
59
|
+
self._joints[j].target = t
|
|
60
|
+
self._joints[j].vlim = vlim
|
|
61
|
+
self._targets_valid = True
|
|
62
|
+
self._send_hold_once()
|
|
63
|
+
if not wait:
|
|
64
|
+
return R1State.NORMAL
|
|
65
|
+
return self._wait_reach(idx, timeout, vlim)
|
|
66
|
+
|
|
67
|
+
@r1_catch("set")
|
|
68
|
+
@r1_ready("set")
|
|
69
|
+
def set_servo_angle_j(self, angles, kp: Optional[Sequence[float]] = None,
|
|
70
|
+
kd: Optional[Sequence[float]] = None, is_radian: Optional[bool] = None) -> int:
|
|
71
|
+
"""Servo mode (set_mode(1), MIT): only the latest target is kept; the keep-alive thread sends it at
|
|
72
|
+
config.keepalive_hz. kp/kd can override the per-joint gains (kd encoding caps at 5). Use with start_control_loop."""
|
|
73
|
+
if self._mode != ArmMode.SERVO:
|
|
74
|
+
self._set_last_error("set_servo_angle_j: needs set_mode(1)")
|
|
75
|
+
return R1State.MODE_MISMATCH
|
|
76
|
+
rad = self._is_radian if is_radian is None else bool(is_radian)
|
|
77
|
+
if not is_number_seq(angles, NJ):
|
|
78
|
+
return R1State.PARAM_ERROR
|
|
79
|
+
tgt = [to_rad(a, rad) for a in angles]
|
|
80
|
+
with self._lock:
|
|
81
|
+
code = self._check_targets(list(range(NJ)), tgt, rad)
|
|
82
|
+
if code != R1State.NORMAL:
|
|
83
|
+
return code
|
|
84
|
+
if kp is not None:
|
|
85
|
+
if not is_number_seq(kp, NJ):
|
|
86
|
+
return R1State.PARAM_ERROR
|
|
87
|
+
for j in range(NJ):
|
|
88
|
+
self._joints[j].kp = max(0.0, min(dm.KP_MAX, float(kp[j])))
|
|
89
|
+
if kd is not None:
|
|
90
|
+
if not is_number_seq(kd, NJ):
|
|
91
|
+
return R1State.PARAM_ERROR
|
|
92
|
+
for j in range(NJ):
|
|
93
|
+
self._joints[j].kd = max(0.0, min(dm.KD_MAX, float(kd[j])))
|
|
94
|
+
for j, t in enumerate(tgt):
|
|
95
|
+
self._joints[j].target = t
|
|
96
|
+
self._targets_valid = True
|
|
97
|
+
return R1State.NORMAL
|
|
98
|
+
|
|
99
|
+
@r1_catch("set")
|
|
100
|
+
@r1_ready("set")
|
|
101
|
+
def vc_set_joint_velocity(self, speeds, duration: float = -1, is_radian: Optional[bool] = None) -> int:
|
|
102
|
+
"""Velocity mode (set_mode(4)). With duration>0 the speed is zeroed automatically when it expires;
|
|
103
|
+
-1 keeps it until the next command."""
|
|
104
|
+
if self._mode != ArmMode.VELOCITY:
|
|
105
|
+
self._set_last_error("vc_set_joint_velocity: needs set_mode(4)")
|
|
106
|
+
return R1State.MODE_MISMATCH
|
|
107
|
+
rad = self._is_radian if is_radian is None else bool(is_radian)
|
|
108
|
+
if not is_number_seq(speeds, NJ):
|
|
109
|
+
return R1State.PARAM_ERROR
|
|
110
|
+
with self._lock:
|
|
111
|
+
for j, v in enumerate(speeds):
|
|
112
|
+
vr = to_rad(v, rad)
|
|
113
|
+
lim = dm.R1_JOINT_LIMITS[j].vmax
|
|
114
|
+
self._joints[j].vel_target = max(-lim, min(lim, vr))
|
|
115
|
+
self._vel_until = (time.monotonic() + duration) if duration and duration > 0 else 0.0
|
|
116
|
+
self._targets_valid = True
|
|
117
|
+
return R1State.NORMAL
|
|
118
|
+
|
|
119
|
+
@r1_catch("set")
|
|
120
|
+
@r1_ready("set")
|
|
121
|
+
def move_gohome(self, speed: Optional[float] = None, wait: bool = True, timeout: Optional[float] = None) -> int:
|
|
122
|
+
return self.set_servo_angle(angle=[0.0] * NJ, speed=speed, is_radian=True, wait=wait, timeout=timeout)
|
|
123
|
+
|
|
124
|
+
# ------------------------------------------------------------------ reading
|
|
125
|
+
@r1_connected("get")
|
|
126
|
+
def get_servo_angle(self, servo_id: Optional[int] = None, is_radian: Optional[bool] = None):
|
|
127
|
+
"""(code, angles[6]) or (code, angle). Uses the cached latest feedback;
|
|
128
|
+
when disabled it automatically requests one feedback with a 0x7FF refresh frame (rate-limited to 5 Hz).
|
|
129
|
+
If the feedback is stale (> config.feedback_stale_s) code=FB_LOST is returned but the old value is still given."""
|
|
130
|
+
rad = self._is_radian if is_radian is None else bool(is_radian)
|
|
131
|
+
idx = self._joint_indices(servo_id)
|
|
132
|
+
if idx is None:
|
|
133
|
+
return R1State.SERVO_NOT_EXIST, None
|
|
134
|
+
self._maybe_poke(idx)
|
|
135
|
+
with self._lock:
|
|
136
|
+
if servo_id is None:
|
|
137
|
+
vals = seq_from_rad([j.pos for j in self._joints], rad)
|
|
138
|
+
else:
|
|
139
|
+
vals = from_rad(self._joints[idx[0]].pos, rad)
|
|
140
|
+
return (R1State.NORMAL if self._joints_fresh(idx) else R1State.FB_LOST), vals
|
|
141
|
+
|
|
142
|
+
@r1_connected("get")
|
|
143
|
+
def get_joint_states(self, is_radian: Optional[bool] = None):
|
|
144
|
+
"""(code, {'pos': [6], 'vel': [6], 'tau': [6], 'temp': [6], 'status': [6]}).
|
|
145
|
+
pos/vel follow is_radian; tau in Nm; temp is MOS temperature in degC; status is the Damiao status nibble
|
|
146
|
+
(0 disabled, 1 enabled, 8+ fault)."""
|
|
147
|
+
rad = self._is_radian if is_radian is None else bool(is_radian)
|
|
148
|
+
self._maybe_poke(None)
|
|
149
|
+
with self._lock:
|
|
150
|
+
out = {
|
|
151
|
+
"pos": seq_from_rad([j.pos for j in self._joints], rad),
|
|
152
|
+
"vel": seq_from_rad([j.vel for j in self._joints], rad),
|
|
153
|
+
"tau": [j.tau for j in self._joints],
|
|
154
|
+
"temp": [j.t_mos for j in self._joints],
|
|
155
|
+
"status": [j.status for j in self._joints],
|
|
156
|
+
}
|
|
157
|
+
return (R1State.NORMAL if self._joints_fresh() else R1State.FB_LOST), out
|
|
158
|
+
|
|
159
|
+
@r1_catch("set")
|
|
160
|
+
@r1_connected("set")
|
|
161
|
+
def request_feedback(self) -> int:
|
|
162
|
+
"""Actively request one feedback from all six joints (0x7FF 0xCC refresh frame; not a register write, so the
|
|
163
|
+
board does not treat it as a takeover). Returns HOST_BLOCKED straight away when blocked, to avoid a wasted send."""
|
|
164
|
+
if self._blocked:
|
|
165
|
+
return R1State.HOST_BLOCKED
|
|
166
|
+
self._poke_feedback(force=True)
|
|
167
|
+
return R1State.NORMAL
|
|
168
|
+
|
|
169
|
+
# ------------------------------------------------------------------ limits / zero
|
|
170
|
+
def set_joint_limits(self, limits, is_radian: Optional[bool] = None) -> int:
|
|
171
|
+
"""Soft limits [(lo, hi)] x 6; pass None to disable. Overrides config.joint_limits."""
|
|
172
|
+
if limits is None:
|
|
173
|
+
self._limits = None
|
|
174
|
+
return R1State.NORMAL
|
|
175
|
+
rad = self._is_radian if is_radian is None else bool(is_radian)
|
|
176
|
+
try:
|
|
177
|
+
lim = [(to_rad(lo, rad), to_rad(hi, rad)) for lo, hi in limits]
|
|
178
|
+
except Exception: # noqa: BLE001
|
|
179
|
+
return R1State.PARAM_ERROR
|
|
180
|
+
if len(lim) != NJ or any(lo >= hi for lo, hi in lim):
|
|
181
|
+
return R1State.PARAM_ERROR
|
|
182
|
+
self._limits = lim
|
|
183
|
+
return R1State.NORMAL
|
|
184
|
+
|
|
185
|
+
def get_joint_limits(self, is_radian: Optional[bool] = None):
|
|
186
|
+
rad = self._is_radian if is_radian is None else bool(is_radian)
|
|
187
|
+
if self._limits is None:
|
|
188
|
+
return R1State.NORMAL, None
|
|
189
|
+
return R1State.NORMAL, [(from_rad(lo, rad), from_rad(hi, rad)) for lo, hi in self._limits]
|
|
190
|
+
|
|
191
|
+
@r1_catch("set")
|
|
192
|
+
@r1_connected("set")
|
|
193
|
+
def set_joint_zero(self, servo_id: Optional[int] = None, timeout: Optional[float] = None) -> int:
|
|
194
|
+
"""Set the current mechanical position as the motor zero (Damiao FE frame, stored in the motor, survives
|
|
195
|
+
power-off). Use after assembly / motor replacement.
|
|
196
|
+
motion_enable(False) must be called first so the arm is disabled and placed at the zero pose;
|
|
197
|
+
on success the feedback position should be near 0, otherwise RES_TIMEOUT is returned."""
|
|
198
|
+
idx = self._joint_indices(servo_id)
|
|
199
|
+
if idx is None:
|
|
200
|
+
return R1State.SERVO_NOT_EXIST
|
|
201
|
+
if self._enabled:
|
|
202
|
+
self._set_last_error("set_joint_zero: call motion_enable(False) first, zero can only be set while disabled")
|
|
203
|
+
return R1State.NOT_READY
|
|
204
|
+
if self._blocked:
|
|
205
|
+
return R1State.HOST_BLOCKED
|
|
206
|
+
code = R1State.NORMAL
|
|
207
|
+
for j in idx:
|
|
208
|
+
c = self._send_ctrl(self._joints[j].esc_id, dm.CTRL_SET_ZERO, confirm=True)
|
|
209
|
+
if c != R1State.NORMAL:
|
|
210
|
+
code = c
|
|
211
|
+
time.sleep(0.02)
|
|
212
|
+
if code != R1State.NORMAL:
|
|
213
|
+
return code
|
|
214
|
+
# take now0 before the pause: on Windows monotonic() has ~15.6 ms granularity, so taking it right next to
|
|
215
|
+
# the poke could give the same tick as the feedback timestamp
|
|
216
|
+
now0 = time.monotonic()
|
|
217
|
+
time.sleep(0.05)
|
|
218
|
+
self._poke_feedback(force=True)
|
|
219
|
+
tmo = self._cfg.timeout if timeout is None else float(timeout)
|
|
220
|
+
ok = self._wait_until(
|
|
221
|
+
lambda: all(self._joints[j].t >= now0 and abs(self._joints[j].pos) <= ZERO_TOL_RAD for j in idx), tmo)
|
|
222
|
+
if not ok:
|
|
223
|
+
self._set_last_error("set_joint_zero: no feedback close to 0 after setting zero")
|
|
224
|
+
return R1State.RES_TIMEOUT
|
|
225
|
+
self._reseed_targets()
|
|
226
|
+
return R1State.NORMAL
|
|
227
|
+
|
|
228
|
+
# ------------------------------------------------------------------ registers (advanced)
|
|
229
|
+
@r1_catch("get")
|
|
230
|
+
@r1_connected("get")
|
|
231
|
+
def get_servo_register(self, servo_id: int, rid: int, timeout: Optional[float] = None):
|
|
232
|
+
"""Read a motor register: (code, value). RIDs are damiao.RID_*; integer RIDs return int, the rest float."""
|
|
233
|
+
idx = self._joint_indices(servo_id)
|
|
234
|
+
if idx is None or servo_id is None:
|
|
235
|
+
return R1State.SERVO_NOT_EXIST, None
|
|
236
|
+
return self._reg_read(self._joints[idx[0]].esc_id, rid, timeout)
|
|
237
|
+
|
|
238
|
+
@r1_catch("set")
|
|
239
|
+
@r1_connected("set")
|
|
240
|
+
def set_servo_register(self, servo_id: int, rid: int, value) -> int:
|
|
241
|
+
"""Write a motor register (not saved to flash, reverts on power-off). Note: a 0x7FF write counts as
|
|
242
|
+
"intent to take over" and will grab control back from the HOLD state."""
|
|
243
|
+
idx = self._joint_indices(servo_id)
|
|
244
|
+
if idx is None or servo_id is None:
|
|
245
|
+
return R1State.SERVO_NOT_EXIST
|
|
246
|
+
return self._reg_write(self._joints[idx[0]].esc_id, rid, value)
|
|
247
|
+
|
|
248
|
+
# ------------------------------------------------------------------ internal
|
|
249
|
+
def _check_targets(self, idx: List[int], tgt: List[float], rad: bool) -> int:
|
|
250
|
+
for j, t in zip(idx, tgt):
|
|
251
|
+
if abs(t) > POS_SANE_RAD:
|
|
252
|
+
return R1State.OUT_OF_RANGE
|
|
253
|
+
if self._limits is not None:
|
|
254
|
+
lo, hi = self._limits[j]
|
|
255
|
+
if not (lo <= t <= hi):
|
|
256
|
+
self._set_last_error(
|
|
257
|
+
f"joint{j + 1} target {from_rad(t, rad):.2f} is outside the limits "
|
|
258
|
+
f"[{from_rad(lo, rad):.1f}, {from_rad(hi, rad):.1f}]")
|
|
259
|
+
return R1State.JOINT_LIMIT
|
|
260
|
+
return R1State.NORMAL
|
|
261
|
+
|
|
262
|
+
def _maybe_poke(self, idx: Optional[List[int]]) -> None:
|
|
263
|
+
if not self._enabled and not self._joints_fresh(idx):
|
|
264
|
+
self._poke_feedback()
|
|
265
|
+
|
|
266
|
+
def _wait_reach(self, idx: List[int], timeout: Optional[float], vlim: float) -> int:
|
|
267
|
+
with self._lock:
|
|
268
|
+
dist = max(abs(self._joints[j].target - self._joints[j].pos) for j in idx)
|
|
269
|
+
tmo = (dist / vlim + 2.0) if timeout is None else float(timeout)
|
|
270
|
+
deadline = time.monotonic() + tmo
|
|
271
|
+
stale = self._cfg.feedback_stale_s
|
|
272
|
+
while time.monotonic() < deadline:
|
|
273
|
+
code = self._ready_code()
|
|
274
|
+
if code != R1State.NORMAL:
|
|
275
|
+
return code
|
|
276
|
+
now = time.monotonic()
|
|
277
|
+
with self._lock:
|
|
278
|
+
done = all(
|
|
279
|
+
self._joints[j].fresh(now, stale)
|
|
280
|
+
and abs(self._joints[j].target - self._joints[j].pos) <= REACH_TOL_RAD
|
|
281
|
+
and abs(self._joints[j].vel) <= REACH_VEL_RAD
|
|
282
|
+
for j in idx
|
|
283
|
+
)
|
|
284
|
+
if done:
|
|
285
|
+
return R1State.NORMAL
|
|
286
|
+
time.sleep(0.02)
|
|
287
|
+
return R1State.WAIT_FINISH_TIMEOUT
|