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
r1_python_sdk/base.py
ADDED
|
@@ -0,0 +1,1075 @@
|
|
|
1
|
+
"""Base: connection, background threads, send / receive, state derivation, enable / mode, callbacks, user control loop.
|
|
2
|
+
|
|
3
|
+
The other mixins (servo / gripper / board / kinematics) only use the internal methods exposed here and never touch
|
|
4
|
+
the serial port themselves.
|
|
5
|
+
|
|
6
|
+
Control-ownership model (see bottom-board firmware r1_arm.c / r1_usb_can.c):
|
|
7
|
+
- When the board is NORMAL the PC's motion frames pass straight to CAN; in TEACH/REPLAY/HOLD/ESTOP they are all
|
|
8
|
+
dropped and answered with TX_FAIL.
|
|
9
|
+
- Only enable / disable / clear-error / set-zero control frames (FF..Fx) and 0x7FF register **writes** (0x55) count
|
|
10
|
+
as "intent to take over" and make the board hand control back from HOLD. Position frames and 0x7FF read / refresh
|
|
11
|
+
do not. So motion_enable(True) is the takeover action; request_feedback() uses a 0x7FF 0xCC refresh and never
|
|
12
|
+
takes over by accident.
|
|
13
|
+
- E-stop (board mushroom button): pressed while the PC is in control, the board switches each joint to MIT and pins
|
|
14
|
+
it; after reset the board stays in HOLD and the PC must call motion_enable(True) again to regain control.
|
|
15
|
+
- The gripper motor (ID 7) belongs to the end board; frames the PC sends to it directly are dropped by the bottom
|
|
16
|
+
board. The SDK uses 0x7F1 command frames instead.
|
|
17
|
+
|
|
18
|
+
Board status (HOLD/TEACH/REPLAY/ESTOP, end-effector buttons, events) comes from the local reports 0x20~0x24 on the
|
|
19
|
+
USB-CAN bridge, so no second cable is needed; the USART2 log port ``$S/$E`` can still be connected and feeds the same
|
|
20
|
+
BoardStatus/BoardEvent.
|
|
21
|
+
"""
|
|
22
|
+
|
|
23
|
+
from __future__ import annotations
|
|
24
|
+
|
|
25
|
+
import math
|
|
26
|
+
import threading
|
|
27
|
+
import time
|
|
28
|
+
from dataclasses import dataclass
|
|
29
|
+
from typing import Callable, Dict, List, Optional, Tuple
|
|
30
|
+
|
|
31
|
+
from . import damiao as dm
|
|
32
|
+
from . import protocol as pr
|
|
33
|
+
from .code import ArmMode, ArmState, R1Error, R1State, R1Warn
|
|
34
|
+
from .config import ConfigSource, R1Config
|
|
35
|
+
from .decorator import r1_catch, r1_connected
|
|
36
|
+
from .logport import LogPort
|
|
37
|
+
from .transport import (REP_HEARTBEAT, REP_LOCAL_EVENT, REP_LOCAL_POS, REP_LOCAL_STATUS,
|
|
38
|
+
REP_LOCAL_TAU, REP_LOCAL_TAUG, REP_LOCAL_VERSION, REP_RX_OK, REP_TX_FAIL, REP_TX_OK,
|
|
39
|
+
CanFrame, DmUsbCan, is_local_report)
|
|
40
|
+
from .utils import find_one_bottom_board, seq_from_rad
|
|
41
|
+
from .version import __version__
|
|
42
|
+
|
|
43
|
+
NJ = pr.NJ
|
|
44
|
+
|
|
45
|
+
BOARD_STALE_S = 0.5 # board status (local report / $S) older than this counts as absent
|
|
46
|
+
BLOCKED_STREAK = 6 # N consecutive TX_FAIL on motion frames => considered blocked
|
|
47
|
+
STOPPING_S = 0.5 # how long to stay in STOPPING after emergency_stop
|
|
48
|
+
POKE_MIN_GAP_S = 0.2 # minimum gap between automatic feedback refreshes while disabled
|
|
49
|
+
MOVING_VEL_RAD = 0.03
|
|
50
|
+
POS_SANE_RAD = 6.3
|
|
51
|
+
|
|
52
|
+
_MODE_TO_DM = {ArmMode.POSITION: dm.MODE_POS_VEL, ArmMode.SERVO: dm.MODE_MIT, ArmMode.VELOCITY: dm.MODE_VEL}
|
|
53
|
+
|
|
54
|
+
ControlFn = Callable[["Base", float], None]
|
|
55
|
+
|
|
56
|
+
|
|
57
|
+
@dataclass
|
|
58
|
+
class JointState:
|
|
59
|
+
"""Latest feedback + command of one joint. Angles are always rad."""
|
|
60
|
+
index: int # 0..5
|
|
61
|
+
esc_id: int # CAN ID
|
|
62
|
+
pos: float = 0.0
|
|
63
|
+
vel: float = 0.0
|
|
64
|
+
tau: float = 0.0
|
|
65
|
+
status: int = 0 # Damiao status nibble
|
|
66
|
+
t_mos: int = 0
|
|
67
|
+
t_rotor: int = 0
|
|
68
|
+
t: float = 0.0 # time of latest feedback, monotonic; 0 = never received
|
|
69
|
+
dm_mode: int = 0 # last confirmed control mode, 0 = unknown
|
|
70
|
+
target: float = 0.0
|
|
71
|
+
vlim: float = 0.5
|
|
72
|
+
vel_target: float = 0.0
|
|
73
|
+
kp: float = 20.0
|
|
74
|
+
kd: float = 1.0
|
|
75
|
+
|
|
76
|
+
def fresh(self, now: float, within: float) -> bool:
|
|
77
|
+
return self.t > 0.0 and (now - self.t) <= within
|
|
78
|
+
|
|
79
|
+
|
|
80
|
+
class Base:
|
|
81
|
+
def __init__(self, port: Optional[str] = None, log_port: Optional[str] = None,
|
|
82
|
+
is_radian: bool = False, do_not_open: bool = False,
|
|
83
|
+
config: ConfigSource = None,
|
|
84
|
+
baud: int = pr.BOTTOM_CDC_BAUD, log_baud: int = pr.LOG_BAUD) -> None:
|
|
85
|
+
self._cfg = R1Config.load(config)
|
|
86
|
+
self._port = port
|
|
87
|
+
self._log_port = log_port
|
|
88
|
+
self._baud = baud
|
|
89
|
+
self._log_baud = log_baud
|
|
90
|
+
self._is_radian = bool(is_radian)
|
|
91
|
+
|
|
92
|
+
self._bus: Optional[DmUsbCan] = None
|
|
93
|
+
self._log: Optional[LogPort] = None
|
|
94
|
+
self._lock = threading.RLock()
|
|
95
|
+
self._cv = threading.Condition(self._lock)
|
|
96
|
+
self._last_error = ""
|
|
97
|
+
|
|
98
|
+
kp, kd = self._cfg.kp_list(), self._cfg.kd_list()
|
|
99
|
+
self._joints: List[JointState] = [
|
|
100
|
+
JointState(index=j, esc_id=int(self._cfg.esc_ids[j]), kp=kp[j], kd=kd[j],
|
|
101
|
+
vlim=math.radians(self._cfg.speed_default))
|
|
102
|
+
for j in range(NJ)
|
|
103
|
+
]
|
|
104
|
+
self._limits: Optional[List[Tuple[float, float]]] = None
|
|
105
|
+
if self._cfg.joint_limits is not None:
|
|
106
|
+
self._limits = [(math.radians(lo), math.radians(hi)) for lo, hi in self._cfg.joint_limits]
|
|
107
|
+
|
|
108
|
+
self._enabled = False
|
|
109
|
+
self._mode = ArmMode.POSITION
|
|
110
|
+
self._targets_valid = False
|
|
111
|
+
self._stopping_until = 0.0
|
|
112
|
+
self._vel_until = 0.0
|
|
113
|
+
|
|
114
|
+
self._blocked = False
|
|
115
|
+
self._tx_fail_streak = 0
|
|
116
|
+
self._tx_results: Dict[int, Tuple[int, float]] = {} # can_id -> (cmd, t)
|
|
117
|
+
self._reg_replies: Dict[Tuple[int, int, int], dm.RegReply] = {}
|
|
118
|
+
self._heartbeat_t = 0.0
|
|
119
|
+
self._last_poke_t = 0.0
|
|
120
|
+
|
|
121
|
+
self._gripper: Optional[pr.GripperStatus] = None
|
|
122
|
+
self._gripper_t = 0.0
|
|
123
|
+
self._board: Optional[pr.BoardStatus] = None
|
|
124
|
+
self._board_version: Optional[pr.BoardVersion] = None
|
|
125
|
+
self._estop_by_event: Optional[bool] = None
|
|
126
|
+
|
|
127
|
+
self._cb: Dict[str, List[Callable]] = {k: [] for k in ("state", "error_warn", "event", "feedback")}
|
|
128
|
+
self._last_state = ArmState.UNKNOWN
|
|
129
|
+
self._last_err_warn = (R1Error.NONE, R1Warn.NONE)
|
|
130
|
+
|
|
131
|
+
self._ka_thread: Optional[threading.Thread] = None
|
|
132
|
+
self._ka_stop = threading.Event()
|
|
133
|
+
|
|
134
|
+
self._ctrl_fn: Optional[ControlFn] = None
|
|
135
|
+
self._ctrl_thread: Optional[threading.Thread] = None
|
|
136
|
+
self._ctrl_stop = threading.Event()
|
|
137
|
+
self._ctrl_rate = float(self._cfg.keepalive_hz)
|
|
138
|
+
|
|
139
|
+
if not do_not_open and (port is not None or find_one_bottom_board() is not None):
|
|
140
|
+
self.connect()
|
|
141
|
+
|
|
142
|
+
# =================================================================== properties
|
|
143
|
+
@property
|
|
144
|
+
def config(self) -> R1Config:
|
|
145
|
+
return self._cfg
|
|
146
|
+
|
|
147
|
+
@property
|
|
148
|
+
def connected(self) -> bool:
|
|
149
|
+
return self._bus is not None and self._bus.is_open
|
|
150
|
+
|
|
151
|
+
@property
|
|
152
|
+
def ready(self) -> bool:
|
|
153
|
+
return self.connected and self._ready_code() == R1State.NORMAL
|
|
154
|
+
|
|
155
|
+
@property
|
|
156
|
+
def motion_enabled(self) -> bool:
|
|
157
|
+
"""Whether the PC really holds control right now: ``motion_enable(True)`` succeeded and the board has not
|
|
158
|
+
blocked it. During teach (blue button) / replay / HOLD / E-stop the board reports HOST_BLOCKED and this
|
|
159
|
+
becomes False immediately -- all keep-alive frames are rejected by the board, and ``motion_enable(True)`` is
|
|
160
|
+
needed to get control back. Also False when disabled / disconnected."""
|
|
161
|
+
return self.connected and self._enabled and not self._blocked
|
|
162
|
+
|
|
163
|
+
@property
|
|
164
|
+
def default_is_radian(self) -> bool:
|
|
165
|
+
return self._is_radian
|
|
166
|
+
|
|
167
|
+
@property
|
|
168
|
+
def version(self) -> str:
|
|
169
|
+
return __version__
|
|
170
|
+
|
|
171
|
+
@property
|
|
172
|
+
def last_error(self) -> str:
|
|
173
|
+
return self._last_error
|
|
174
|
+
|
|
175
|
+
@property
|
|
176
|
+
def state(self) -> int:
|
|
177
|
+
return self._compute_state()
|
|
178
|
+
|
|
179
|
+
@property
|
|
180
|
+
def mode(self) -> int:
|
|
181
|
+
return self._mode
|
|
182
|
+
|
|
183
|
+
@property
|
|
184
|
+
def angles(self) -> List[float]:
|
|
185
|
+
with self._lock:
|
|
186
|
+
return seq_from_rad([j.pos for j in self._joints], self._is_radian)
|
|
187
|
+
|
|
188
|
+
@property
|
|
189
|
+
def joints_torque(self) -> List[float]:
|
|
190
|
+
with self._lock:
|
|
191
|
+
return [j.tau for j in self._joints]
|
|
192
|
+
|
|
193
|
+
@property
|
|
194
|
+
def temperatures(self) -> List[int]:
|
|
195
|
+
with self._lock:
|
|
196
|
+
return [j.t_mos for j in self._joints]
|
|
197
|
+
|
|
198
|
+
@property
|
|
199
|
+
def error_code(self) -> int:
|
|
200
|
+
return self._compute_err_warn()[0]
|
|
201
|
+
|
|
202
|
+
@property
|
|
203
|
+
def warn_code(self) -> int:
|
|
204
|
+
return self._compute_err_warn()[1]
|
|
205
|
+
|
|
206
|
+
@property
|
|
207
|
+
def has_error(self) -> bool:
|
|
208
|
+
return self.error_code != R1Error.NONE
|
|
209
|
+
|
|
210
|
+
@property
|
|
211
|
+
def has_warn(self) -> bool:
|
|
212
|
+
return self.warn_code != R1Warn.NONE
|
|
213
|
+
|
|
214
|
+
@property
|
|
215
|
+
def board_status(self) -> Optional[pr.BoardStatus]:
|
|
216
|
+
"""Latest board status (USB local report or ``$S``); None if none received."""
|
|
217
|
+
return self._board
|
|
218
|
+
|
|
219
|
+
@property
|
|
220
|
+
def board_version(self) -> Optional[pr.BoardVersion]:
|
|
221
|
+
"""Bottom-board firmware version (the one reported on power-up, or obtained via ``get_board_version``).
|
|
222
|
+
None for old firmware / not received yet."""
|
|
223
|
+
return self._board_version
|
|
224
|
+
|
|
225
|
+
@property
|
|
226
|
+
def gripper_status(self) -> Optional[pr.GripperStatus]:
|
|
227
|
+
return self._gripper
|
|
228
|
+
|
|
229
|
+
@property
|
|
230
|
+
def joints(self) -> List[JointState]:
|
|
231
|
+
with self._lock:
|
|
232
|
+
return [JointState(**vars(j)) for j in self._joints]
|
|
233
|
+
|
|
234
|
+
@property
|
|
235
|
+
def bus_stats(self):
|
|
236
|
+
return self._bus.stats if self._bus is not None else None
|
|
237
|
+
|
|
238
|
+
@property
|
|
239
|
+
def control_loop_active(self) -> bool:
|
|
240
|
+
t = self._ctrl_thread
|
|
241
|
+
return t is not None and t.is_alive()
|
|
242
|
+
|
|
243
|
+
# =================================================================== A. connection
|
|
244
|
+
@r1_catch("set")
|
|
245
|
+
def connect(self, port: Optional[str] = None, log_port: Optional[str] = None) -> int:
|
|
246
|
+
"""Connect to the bottom board's USB-CAN bridge; ``log_port`` is the USART2 log port (optional, just an
|
|
247
|
+
additional source of the same status)."""
|
|
248
|
+
if self.connected:
|
|
249
|
+
return R1State.NORMAL
|
|
250
|
+
port = port or self._port or find_one_bottom_board()
|
|
251
|
+
if not port:
|
|
252
|
+
self._set_last_error("connect: no bottom-board CDC port found (VID 0483 PID 7154), or more than one")
|
|
253
|
+
return R1State.NOT_CONNECTED
|
|
254
|
+
bus = DmUsbCan(port, self._baud)
|
|
255
|
+
bus.add_callback(self._on_bus)
|
|
256
|
+
bus.open()
|
|
257
|
+
self._bus = bus
|
|
258
|
+
self._port = port
|
|
259
|
+
|
|
260
|
+
# heartbeat handshake to confirm the other side is the board and not some random serial port
|
|
261
|
+
self._heartbeat_t = 0.0
|
|
262
|
+
bus.heartbeat()
|
|
263
|
+
if not self._wait_until(lambda: self._heartbeat_t > 0.0, 0.5):
|
|
264
|
+
bus.close()
|
|
265
|
+
self._bus = None
|
|
266
|
+
self._set_last_error(f"connect: {port} did not answer the heartbeat")
|
|
267
|
+
return R1State.RES_TIMEOUT
|
|
268
|
+
|
|
269
|
+
log_port = log_port or self._log_port
|
|
270
|
+
if log_port:
|
|
271
|
+
try:
|
|
272
|
+
lp = LogPort(log_port, self._log_baud)
|
|
273
|
+
lp.add_callback(self._on_log)
|
|
274
|
+
lp.open()
|
|
275
|
+
self._log = lp
|
|
276
|
+
self._log_port = log_port
|
|
277
|
+
except Exception as exc: # noqa: BLE001
|
|
278
|
+
self._set_last_error(f"connect: failed to open log port {log_port}: {exc!r} (ignored, continuing)")
|
|
279
|
+
self._log = None
|
|
280
|
+
|
|
281
|
+
self._reset_runtime()
|
|
282
|
+
self._ka_stop.clear()
|
|
283
|
+
self._ka_thread = threading.Thread(target=self._keepalive_loop, name="r1-keepalive", daemon=True)
|
|
284
|
+
self._ka_thread.start()
|
|
285
|
+
# ask for the firmware version once (the power-up 0x25 may have been flushed before the port was opened).
|
|
286
|
+
# old firmware replies TX_FAIL, ignored
|
|
287
|
+
self.get_board_version(timeout=0.3)
|
|
288
|
+
return R1State.NORMAL
|
|
289
|
+
|
|
290
|
+
def disconnect(self) -> int:
|
|
291
|
+
"""Disconnect. Does not disable the motors itself: once keep-alive stops, the motors disable on their own CAN
|
|
292
|
+
TIMEOUT (the whole arm goes limp). To keep the pose, hand control to the board first (blue button -> teach /
|
|
293
|
+
pinned) and then disconnect."""
|
|
294
|
+
self.stop_control_loop()
|
|
295
|
+
self._ka_stop.set()
|
|
296
|
+
t = self._ka_thread
|
|
297
|
+
if t is not None and t.is_alive():
|
|
298
|
+
t.join(1.0)
|
|
299
|
+
self._ka_thread = None
|
|
300
|
+
if self._log is not None:
|
|
301
|
+
self._log.close()
|
|
302
|
+
self._log = None
|
|
303
|
+
if self._bus is not None:
|
|
304
|
+
self._bus.close()
|
|
305
|
+
self._bus = None
|
|
306
|
+
self._enabled = False
|
|
307
|
+
self._targets_valid = False
|
|
308
|
+
return R1State.NORMAL
|
|
309
|
+
|
|
310
|
+
def get_version(self) -> Tuple[int, dict]:
|
|
311
|
+
"""(code, {'sdk': ..., 'firmware': None}). The firmware version query frame is not defined yet (FIRMWARE_PENDING item)."""
|
|
312
|
+
return R1State.NORMAL, {"sdk": __version__, "firmware": None}
|
|
313
|
+
|
|
314
|
+
# =================================================================== B. enable / mode / state
|
|
315
|
+
@r1_catch("set")
|
|
316
|
+
@r1_connected("set")
|
|
317
|
+
def motion_enable(self, enable: bool = True, servo_id: Optional[int] = None) -> int:
|
|
318
|
+
"""Enable / disable. Enable sequence: clear error -> FC -> write CTRL_MODE and confirm -> seed the hold
|
|
319
|
+
targets from feedback -> keep-alive starts sending frames. This is also the action that takes control back
|
|
320
|
+
from the board's HOLD. ``servo_id`` 1~6 acts on a single joint (mostly used when disabling)."""
|
|
321
|
+
idx = self._joint_indices(servo_id)
|
|
322
|
+
if idx is None:
|
|
323
|
+
return R1State.SERVO_NOT_EXIST
|
|
324
|
+
if not enable:
|
|
325
|
+
self.stop_control_loop()
|
|
326
|
+
self._enabled = False
|
|
327
|
+
self._targets_valid = False
|
|
328
|
+
for j in idx:
|
|
329
|
+
self._send_ctrl(self._joints[j].esc_id, dm.CTRL_DISABLE)
|
|
330
|
+
return R1State.NORMAL
|
|
331
|
+
|
|
332
|
+
if self._estop_active():
|
|
333
|
+
return R1State.EMERGENCY_STOP
|
|
334
|
+
|
|
335
|
+
code = R1State.NORMAL
|
|
336
|
+
want = _MODE_TO_DM.get(self._mode)
|
|
337
|
+
for j in idx:
|
|
338
|
+
self._send_ctrl(self._joints[j].esc_id, dm.CTRL_CLEAR_ERR)
|
|
339
|
+
time.sleep(0.01)
|
|
340
|
+
for j in idx:
|
|
341
|
+
c = self._send_ctrl(self._joints[j].esc_id, dm.CTRL_ENABLE, confirm=True)
|
|
342
|
+
if c == R1State.TX_FAIL:
|
|
343
|
+
code = R1State.TX_FAIL
|
|
344
|
+
time.sleep(0.02)
|
|
345
|
+
if want is not None:
|
|
346
|
+
for j in idx:
|
|
347
|
+
c = self._ensure_mode(j, want)
|
|
348
|
+
if c != R1State.NORMAL and code == R1State.NORMAL:
|
|
349
|
+
code = c
|
|
350
|
+
if code == R1State.TX_FAIL:
|
|
351
|
+
self._set_last_error("motion_enable: failed to send the enable frame (bus? blocked?)")
|
|
352
|
+
return code
|
|
353
|
+
|
|
354
|
+
# wait for feedback from all joints (each FC / register write triggers one feedback frame)
|
|
355
|
+
if not self._wait_until(lambda: all(self._joints[j].fresh(time.monotonic(), 0.3) for j in idx), 0.5):
|
|
356
|
+
self._set_last_error("motion_enable: no feedback from some joints")
|
|
357
|
+
if code == R1State.NORMAL:
|
|
358
|
+
code = R1State.FB_LOST
|
|
359
|
+
self._reseed_targets()
|
|
360
|
+
self._enabled = True
|
|
361
|
+
self._stopping_until = 0.0
|
|
362
|
+
self._blocked = False
|
|
363
|
+
self._tx_fail_streak = 0
|
|
364
|
+
self._send_hold_once()
|
|
365
|
+
return code
|
|
366
|
+
|
|
367
|
+
@r1_connected("get")
|
|
368
|
+
def get_state(self) -> Tuple[int, int]:
|
|
369
|
+
"""(code, state); state values are in ArmState."""
|
|
370
|
+
return R1State.NORMAL, self._compute_state()
|
|
371
|
+
|
|
372
|
+
def get_mode(self) -> Tuple[int, int]:
|
|
373
|
+
return R1State.NORMAL, self._mode
|
|
374
|
+
|
|
375
|
+
@r1_catch("set")
|
|
376
|
+
@r1_connected("set")
|
|
377
|
+
def set_mode(self, mode: int = 0) -> int:
|
|
378
|
+
"""0 position / 1 servo (MIT) / 4 velocity. When already enabled, switches the motors' CTRL_MODE online and
|
|
379
|
+
re-seeds the targets. 2 teach is a board state that can only be entered with the end-effector blue button;
|
|
380
|
+
the PC cannot set it (get_state() shows TEACHING)."""
|
|
381
|
+
if mode == ArmMode.TEACH:
|
|
382
|
+
self._set_last_error("set_mode(2): teach is entered with the end-effector blue button, the PC cannot set it")
|
|
383
|
+
return R1State.CMD_NOT_EXIST
|
|
384
|
+
if mode not in _MODE_TO_DM:
|
|
385
|
+
return R1State.PARAM_ERROR
|
|
386
|
+
if mode == self._mode:
|
|
387
|
+
return R1State.NORMAL
|
|
388
|
+
if self._enabled:
|
|
389
|
+
if self.get_is_moving():
|
|
390
|
+
self._set_last_error("set_mode: cannot switch mode while moving (manual: commands are cleared on a switch)")
|
|
391
|
+
return R1State.NOT_READY
|
|
392
|
+
self._targets_valid = False
|
|
393
|
+
code = R1State.NORMAL
|
|
394
|
+
for j in range(NJ):
|
|
395
|
+
c = self._ensure_mode(j, _MODE_TO_DM[mode])
|
|
396
|
+
if c != R1State.NORMAL:
|
|
397
|
+
code = c
|
|
398
|
+
self._mode = mode
|
|
399
|
+
self._reseed_targets()
|
|
400
|
+
return code
|
|
401
|
+
self._mode = mode
|
|
402
|
+
return R1State.NORMAL
|
|
403
|
+
|
|
404
|
+
@r1_connected("get")
|
|
405
|
+
def get_err_warn_code(self) -> Tuple[int, List[int]]:
|
|
406
|
+
"""(code, [err, warn]). err values are in R1Error, warn values in R1Warn."""
|
|
407
|
+
e, w = self._compute_err_warn()
|
|
408
|
+
return R1State.NORMAL, [e, w]
|
|
409
|
+
|
|
410
|
+
@staticmethod
|
|
411
|
+
def error_text(err: int) -> str:
|
|
412
|
+
if err == R1Error.NONE:
|
|
413
|
+
return "none"
|
|
414
|
+
if err == R1Error.ESTOP:
|
|
415
|
+
return "E-stop"
|
|
416
|
+
if err == R1Error.FB_LOST:
|
|
417
|
+
return "feedback lost"
|
|
418
|
+
if err == R1Error.BOARD_ALARM:
|
|
419
|
+
return "board alarm"
|
|
420
|
+
if R1Error.is_motor(err):
|
|
421
|
+
j, st = R1Error.motor_joint(err), R1Error.motor_status(err)
|
|
422
|
+
return f"joint{j} {dm.STATUS_NAMES.get(st, hex(st))}"
|
|
423
|
+
return f"error {err}"
|
|
424
|
+
|
|
425
|
+
@r1_catch("set")
|
|
426
|
+
@r1_connected("set")
|
|
427
|
+
def clean_error(self) -> int:
|
|
428
|
+
"""Send a clear-error frame to all six joints. Note: the clear-error frame counts as "intent to take over";
|
|
429
|
+
a board in HOLD will hand control back to the PC."""
|
|
430
|
+
if self._blocked and not self._enabled:
|
|
431
|
+
self._set_last_error("clean_error: refused, the board is in local control; clearing errors would take "
|
|
432
|
+
"control back with nobody holding the arm")
|
|
433
|
+
return R1State.HOST_BLOCKED
|
|
434
|
+
for j in self._joints:
|
|
435
|
+
self._send_ctrl(j.esc_id, dm.CTRL_CLEAR_ERR)
|
|
436
|
+
return R1State.NORMAL
|
|
437
|
+
|
|
438
|
+
@r1_catch("set")
|
|
439
|
+
@r1_connected("set")
|
|
440
|
+
def emergency_stop(self) -> int:
|
|
441
|
+
"""Software E-stop: stops immediately in any state, motors keep torque (R1 carries a gravity load; an E-stop
|
|
442
|
+
that disables would let the arm crash down):
|
|
443
|
+
|
|
444
|
+
- PC in control (NORMAL): stop the user control loop, pin the six joint targets at the current position,
|
|
445
|
+
STOPPING for 0.5 s then back to READY; motion commands are rejected meanwhile.
|
|
446
|
+
- Board in control (teach / replay / HOLD / E-stop, PC blocked): none of the PC's motion frames get through,
|
|
447
|
+
so send the local command 0x7F4 PAUSE instead; the board enters HOLD and pins, just like releasing the
|
|
448
|
+
blue button / pressing green. Success only when the board replies TX_OK; afterwards get_state() = PAUSED
|
|
449
|
+
and motion_enable(True) is needed to regain control. Firmware that does not know this command (too old)
|
|
450
|
+
times out and RES_TIMEOUT is returned; then only the end-effector buttons or the mushroom button can help.
|
|
451
|
+
|
|
452
|
+
The real hard E-stop is still the board's mushroom button: the board switches each joint to MIT, pins it
|
|
453
|
+
and blocks the PC."""
|
|
454
|
+
self.stop_control_loop()
|
|
455
|
+
if self._blocked:
|
|
456
|
+
code = self._send(CanFrame(pr.CAN_ID_HOST_CMD, bytes([pr.HOST_CMD_PAUSE])),
|
|
457
|
+
confirm=True, timeout=0.3)
|
|
458
|
+
if code == R1State.HOST_BLOCKED:
|
|
459
|
+
code = R1State.TX_FAIL # this frame is not subject to blocking; FAIL means "the board is not in charge in the current state"
|
|
460
|
+
if code != R1State.NORMAL:
|
|
461
|
+
self._set_last_error(f"emergency_stop: the board did not confirm the pause ({R1State.text(code)})")
|
|
462
|
+
return code
|
|
463
|
+
self._hold_here()
|
|
464
|
+
self._stopping_until = time.monotonic() + STOPPING_S
|
|
465
|
+
return R1State.NORMAL
|
|
466
|
+
|
|
467
|
+
def request_board_pause(self) -> int:
|
|
468
|
+
"""Only send 0x7F4 PAUSE without touching the PC-side targets (this is what ``emergency_stop`` does when
|
|
469
|
+
blocked). In NORMAL state the board replies TX_FAIL -- the PC is in control itself, just use ``emergency_stop``."""
|
|
470
|
+
if not self.connected:
|
|
471
|
+
return R1State.NOT_CONNECTED
|
|
472
|
+
code = self._send(CanFrame(pr.CAN_ID_HOST_CMD, bytes([pr.HOST_CMD_PAUSE])),
|
|
473
|
+
confirm=True, timeout=0.3)
|
|
474
|
+
return R1State.TX_FAIL if code == R1State.HOST_BLOCKED else code
|
|
475
|
+
|
|
476
|
+
def get_board_version(self, timeout: float = 0.3) -> Optional[pr.BoardVersion]:
|
|
477
|
+
"""Ask the board for its firmware version once (0x7F4 GET_VERSION -> 0x25); works even while blocked.
|
|
478
|
+
|
|
479
|
+
The board sends 0x25 before replying TX_OK, so by the time the reply arrives the version is already in
|
|
480
|
+
``board_version``. Old firmware does not know the command, replies TX_FAIL, and None is returned
|
|
481
|
+
(``board_version`` stays None too)."""
|
|
482
|
+
if not self.connected:
|
|
483
|
+
return None
|
|
484
|
+
code = self._send(CanFrame(pr.CAN_ID_HOST_CMD, bytes([pr.HOST_CMD_GET_VERSION])),
|
|
485
|
+
confirm=True, timeout=timeout)
|
|
486
|
+
return self._board_version if code == R1State.NORMAL else None
|
|
487
|
+
|
|
488
|
+
def get_is_moving(self) -> bool:
|
|
489
|
+
now = time.monotonic()
|
|
490
|
+
with self._lock:
|
|
491
|
+
return any(j.fresh(now, self._cfg.feedback_stale_s) and abs(j.vel) > MOVING_VEL_RAD
|
|
492
|
+
for j in self._joints)
|
|
493
|
+
|
|
494
|
+
# =================================================================== callbacks
|
|
495
|
+
def register_state_changed_callback(self, cb: Callable[[dict], None]) -> bool:
|
|
496
|
+
"""``{'state': ArmState}``. Also fires when the user enters TEACH/REPLAY with the blue / green button."""
|
|
497
|
+
return self._reg("state", cb)
|
|
498
|
+
|
|
499
|
+
def release_state_changed_callback(self, cb=None) -> bool:
|
|
500
|
+
return self._unreg("state", cb)
|
|
501
|
+
|
|
502
|
+
def register_error_warn_changed_callback(self, cb: Callable[[dict], None]) -> bool:
|
|
503
|
+
"""``{'error_code': R1Error, 'warn_code': R1Warn}``."""
|
|
504
|
+
return self._reg("error_warn", cb)
|
|
505
|
+
|
|
506
|
+
def release_error_warn_changed_callback(self, cb=None) -> bool:
|
|
507
|
+
return self._unreg("error_warn", cb)
|
|
508
|
+
|
|
509
|
+
def register_event_callback(self, cb: Callable[[pr.BoardEvent], None]) -> bool:
|
|
510
|
+
"""Board events BoardEvent (TRAJ_SAVED / REPLAY_DONE / ESTOP_ON ...), from USB local report 0x21 or the log port ``$E``."""
|
|
511
|
+
return self._reg("event", cb)
|
|
512
|
+
|
|
513
|
+
def release_event_callback(self, cb=None) -> bool:
|
|
514
|
+
return self._unreg("event", cb)
|
|
515
|
+
|
|
516
|
+
def register_feedback_callback(self, cb: Callable[[JointState], None]) -> bool:
|
|
517
|
+
"""Once per motor feedback frame, for logging / plotting. Called on the receive thread, so keep it fast."""
|
|
518
|
+
return self._reg("feedback", cb)
|
|
519
|
+
|
|
520
|
+
def release_feedback_callback(self, cb=None) -> bool:
|
|
521
|
+
return self._unreg("feedback", cb)
|
|
522
|
+
|
|
523
|
+
# =================================================================== user control loop
|
|
524
|
+
@r1_catch("set")
|
|
525
|
+
@r1_connected("set")
|
|
526
|
+
def start_control_loop(self, fn: ControlFn, rate: Optional[float] = None) -> int:
|
|
527
|
+
"""Call ``fn(arm, dt)`` repeatedly at ``rate`` Hz on a background thread; inside fn use set_servo_angle_j /
|
|
528
|
+
vc_set_joint_velocity to update the targets. If fn raises, the loop stops and the error is recorded in
|
|
529
|
+
last_error; the keep-alive thread keeps pinning the last target so the arm does not drop.
|
|
530
|
+
rate defaults to config.keepalive_hz."""
|
|
531
|
+
if not callable(fn):
|
|
532
|
+
return R1State.PARAM_ERROR
|
|
533
|
+
if self.control_loop_active:
|
|
534
|
+
self._set_last_error("start_control_loop: already running, call stop_control_loop() first")
|
|
535
|
+
return R1State.NOT_READY
|
|
536
|
+
code = self._ready_code()
|
|
537
|
+
if code != R1State.NORMAL:
|
|
538
|
+
self._set_last_error(f"start_control_loop: {R1State.text(code)}")
|
|
539
|
+
return code
|
|
540
|
+
rate = float(self._cfg.keepalive_hz if rate is None else rate)
|
|
541
|
+
if not (1.0 <= rate <= 1000.0):
|
|
542
|
+
return R1State.OUT_OF_RANGE
|
|
543
|
+
self._ctrl_fn = fn
|
|
544
|
+
self._ctrl_rate = rate
|
|
545
|
+
self._ctrl_stop.clear()
|
|
546
|
+
self._ctrl_thread = threading.Thread(target=self._control_loop_impl, name="r1-control", daemon=True)
|
|
547
|
+
self._ctrl_thread.start()
|
|
548
|
+
return R1State.NORMAL
|
|
549
|
+
|
|
550
|
+
def stop_control_loop(self) -> int:
|
|
551
|
+
self._ctrl_stop.set()
|
|
552
|
+
t = self._ctrl_thread
|
|
553
|
+
if t is not None and t.is_alive() and t is not threading.current_thread():
|
|
554
|
+
t.join(1.0)
|
|
555
|
+
self._ctrl_thread = None
|
|
556
|
+
self._ctrl_fn = None
|
|
557
|
+
return R1State.NORMAL
|
|
558
|
+
|
|
559
|
+
def _control_loop_impl(self) -> None:
|
|
560
|
+
dt = 1.0 / self._ctrl_rate
|
|
561
|
+
next_t = time.monotonic()
|
|
562
|
+
while not self._ctrl_stop.is_set():
|
|
563
|
+
fn = self._ctrl_fn
|
|
564
|
+
if fn is None:
|
|
565
|
+
break
|
|
566
|
+
try:
|
|
567
|
+
fn(self, dt)
|
|
568
|
+
except Exception as exc: # noqa: BLE001
|
|
569
|
+
self._set_last_error(f"control_loop: {exc!r}, loop stopped")
|
|
570
|
+
break
|
|
571
|
+
next_t += dt
|
|
572
|
+
wait = next_t - time.monotonic()
|
|
573
|
+
if wait > 0:
|
|
574
|
+
self._ctrl_stop.wait(wait)
|
|
575
|
+
else:
|
|
576
|
+
next_t = time.monotonic()
|
|
577
|
+
|
|
578
|
+
# =================================================================== internal: state derivation
|
|
579
|
+
def _ready_code(self) -> int:
|
|
580
|
+
if not self._enabled:
|
|
581
|
+
return R1State.NOT_READY
|
|
582
|
+
if self._estop_active():
|
|
583
|
+
return R1State.EMERGENCY_STOP
|
|
584
|
+
if self._blocked:
|
|
585
|
+
return R1State.HOST_BLOCKED
|
|
586
|
+
if time.monotonic() < self._stopping_until:
|
|
587
|
+
return R1State.NOT_READY
|
|
588
|
+
return R1State.NORMAL
|
|
589
|
+
|
|
590
|
+
def _board_fresh(self) -> Optional[pr.BoardStatus]:
|
|
591
|
+
b = self._board
|
|
592
|
+
if b is None or (time.monotonic() - b.t) > BOARD_STALE_S:
|
|
593
|
+
return None
|
|
594
|
+
return b
|
|
595
|
+
|
|
596
|
+
def _estop_active(self) -> bool:
|
|
597
|
+
b = self._board_fresh()
|
|
598
|
+
if b is not None:
|
|
599
|
+
return b.state == pr.ST_ESTOP or b.flag(pr.SF_ESTOP)
|
|
600
|
+
return bool(self._estop_by_event)
|
|
601
|
+
|
|
602
|
+
def _compute_state(self) -> int:
|
|
603
|
+
if not self.connected:
|
|
604
|
+
return ArmState.UNKNOWN
|
|
605
|
+
if self._estop_active():
|
|
606
|
+
return ArmState.ESTOP
|
|
607
|
+
b = self._board_fresh()
|
|
608
|
+
if b is not None:
|
|
609
|
+
if b.state == pr.ST_TEACH:
|
|
610
|
+
return ArmState.TEACHING
|
|
611
|
+
if b.state == pr.ST_REPLAY:
|
|
612
|
+
return ArmState.PLAYBACK
|
|
613
|
+
if b.state == pr.ST_HOLD:
|
|
614
|
+
return ArmState.PAUSED
|
|
615
|
+
if b.state in (pr.ST_BOOT, pr.ST_PROBE, pr.ST_BRINGUP):
|
|
616
|
+
return ArmState.INITIALIZING
|
|
617
|
+
elif self._blocked:
|
|
618
|
+
return ArmState.BLOCKED
|
|
619
|
+
if time.monotonic() < self._stopping_until:
|
|
620
|
+
return ArmState.STOPPING
|
|
621
|
+
return ArmState.MOVING if self.get_is_moving() else ArmState.READY
|
|
622
|
+
|
|
623
|
+
def _compute_err_warn(self) -> Tuple[int, int]:
|
|
624
|
+
now = time.monotonic()
|
|
625
|
+
err = R1Error.NONE
|
|
626
|
+
warn = R1Warn.NONE
|
|
627
|
+
if self._estop_active():
|
|
628
|
+
err = R1Error.ESTOP
|
|
629
|
+
else:
|
|
630
|
+
with self._lock:
|
|
631
|
+
for j in self._joints:
|
|
632
|
+
if j.fresh(now, 2.0) and dm.status_is_fault(j.status):
|
|
633
|
+
err = R1Error.motor(j.index + 1, j.status)
|
|
634
|
+
break
|
|
635
|
+
if err == R1Error.NONE:
|
|
636
|
+
b = self._board_fresh()
|
|
637
|
+
if b is not None:
|
|
638
|
+
if b.flag(pr.SF_ALARM):
|
|
639
|
+
err = R1Error.BOARD_ALARM
|
|
640
|
+
elif b.state == pr.ST_NORMAL and b.fb_mask == 0 and self._enabled:
|
|
641
|
+
err = R1Error.FB_LOST
|
|
642
|
+
if self._blocked:
|
|
643
|
+
warn = R1Warn.HOST_BLOCKED
|
|
644
|
+
elif self._enabled:
|
|
645
|
+
with self._lock:
|
|
646
|
+
if not all(j.fresh(now, self._cfg.feedback_stale_s) for j in self._joints):
|
|
647
|
+
warn = R1Warn.FEEDBACK_STALE
|
|
648
|
+
if warn == R1Warn.NONE:
|
|
649
|
+
g = self._gripper_fresh()
|
|
650
|
+
if g is not None:
|
|
651
|
+
if g.status == pr.GRIP_ST_FAULT:
|
|
652
|
+
warn = R1Warn.GRIPPER_FAULT
|
|
653
|
+
elif not g.homed:
|
|
654
|
+
warn = R1Warn.GRIPPER_NOT_INIT
|
|
655
|
+
return err, warn
|
|
656
|
+
|
|
657
|
+
def _gripper_fresh(self) -> Optional[pr.GripperStatus]:
|
|
658
|
+
if self._gripper is None or (time.monotonic() - self._gripper_t) > 0.5:
|
|
659
|
+
return None
|
|
660
|
+
return self._gripper
|
|
661
|
+
|
|
662
|
+
def _joints_fresh(self, idx: Optional[List[int]] = None) -> bool:
|
|
663
|
+
now = time.monotonic()
|
|
664
|
+
with self._lock:
|
|
665
|
+
js = self._joints if idx is None else [self._joints[j] for j in idx]
|
|
666
|
+
return all(j.fresh(now, self._cfg.feedback_stale_s) for j in js)
|
|
667
|
+
|
|
668
|
+
# =================================================================== internal: sending
|
|
669
|
+
def _send_ctrl(self, esc: int, cmd: int, confirm: bool = False) -> int:
|
|
670
|
+
f = CanFrame(dm.ID_MIT + esc, dm.ctrl_frame(cmd))
|
|
671
|
+
return self._send(f, confirm)
|
|
672
|
+
|
|
673
|
+
def _send(self, f: CanFrame, confirm: bool = False, timeout: float = 0.15) -> int:
|
|
674
|
+
bus = self._bus
|
|
675
|
+
if bus is None:
|
|
676
|
+
return R1State.NOT_CONNECTED
|
|
677
|
+
with self._lock:
|
|
678
|
+
self._tx_results.pop(f.id, None)
|
|
679
|
+
bus.send(f)
|
|
680
|
+
if not confirm:
|
|
681
|
+
return R1State.NORMAL
|
|
682
|
+
if not self._wait_until(lambda: f.id in self._tx_results, timeout):
|
|
683
|
+
return R1State.RES_TIMEOUT
|
|
684
|
+
cmd, _ = self._tx_results[f.id]
|
|
685
|
+
if cmd == REP_TX_OK:
|
|
686
|
+
return R1State.NORMAL
|
|
687
|
+
return R1State.HOST_BLOCKED if self._blocked else R1State.TX_FAIL
|
|
688
|
+
|
|
689
|
+
def _reg_read(self, esc: int, rid: int, timeout: Optional[float]):
|
|
690
|
+
key = (esc, rid, dm.REG_READ)
|
|
691
|
+
with self._lock:
|
|
692
|
+
self._reg_replies.pop(key, None)
|
|
693
|
+
c = self._send(CanFrame(dm.ID_REG, dm.reg_read_frame(esc, rid)), confirm=True)
|
|
694
|
+
if c != R1State.NORMAL:
|
|
695
|
+
return c, None
|
|
696
|
+
tmo = self._cfg.timeout if timeout is None else float(timeout)
|
|
697
|
+
if not self._wait_until(lambda: key in self._reg_replies, tmo):
|
|
698
|
+
return R1State.RES_TIMEOUT, None
|
|
699
|
+
return R1State.NORMAL, self._reg_replies[key].value
|
|
700
|
+
|
|
701
|
+
def _reg_write(self, esc: int, rid: int, value, timeout: Optional[float] = None) -> int:
|
|
702
|
+
key = (esc, rid, dm.REG_WRITE)
|
|
703
|
+
with self._lock:
|
|
704
|
+
self._reg_replies.pop(key, None)
|
|
705
|
+
c = self._send(CanFrame(dm.ID_REG, dm.reg_write_frame(esc, rid, value)), confirm=True)
|
|
706
|
+
if c != R1State.NORMAL:
|
|
707
|
+
return c
|
|
708
|
+
tmo = self._cfg.timeout if timeout is None else float(timeout)
|
|
709
|
+
if not self._wait_until(lambda: key in self._reg_replies, tmo):
|
|
710
|
+
return R1State.RES_TIMEOUT
|
|
711
|
+
return R1State.NORMAL
|
|
712
|
+
|
|
713
|
+
def _ensure_mode(self, j: int, want: int) -> int:
|
|
714
|
+
"""Write CTRL_MODE and confirm with the reply; on success record it in JointState.dm_mode and immediately send
|
|
715
|
+
this joint a "stay here" target frame.
|
|
716
|
+
|
|
717
|
+
As soon as the register is written the motor swaps its internal controller (MIT kp/kd/tau_ff are gone, the
|
|
718
|
+
new loop starts from zero); until the PC's first target frame arrives it has no target, and a loaded joint
|
|
719
|
+
sags a little. The six joints are switched serially, so the whole arm visibly twitches on takeover. Hence
|
|
720
|
+
right after each switch a hold frame of that mode is sent with target = the joint's latest feedback position,
|
|
721
|
+
shrinking the no-target window from hundreds of ms to 1~2 ms."""
|
|
722
|
+
js = self._joints[j]
|
|
723
|
+
code = self._reg_write(js.esc_id, dm.RID_CTRL_MODE, want)
|
|
724
|
+
if code != R1State.NORMAL:
|
|
725
|
+
return code
|
|
726
|
+
rep = self._reg_replies.get((js.esc_id, dm.RID_CTRL_MODE, dm.REG_WRITE))
|
|
727
|
+
got = int(rep.value) if rep is not None else -1
|
|
728
|
+
if got != want:
|
|
729
|
+
self._set_last_error(f"joint{j + 1} CTRL_MODE: wrote {want}, read back {got}")
|
|
730
|
+
return R1State.MODE_MISMATCH
|
|
731
|
+
js.dm_mode = want
|
|
732
|
+
self._send_stay_frame(j, want)
|
|
733
|
+
return R1State.NORMAL
|
|
734
|
+
|
|
735
|
+
def _send_stay_frame(self, j: int, dm_mode: int) -> None:
|
|
736
|
+
"""Send one "stay at the current feedback position" frame to a single joint (frame type chosen by motor mode).
|
|
737
|
+
If the feedback is stale, send nothing -- better not to move."""
|
|
738
|
+
bus = self._bus
|
|
739
|
+
js = self._joints[j]
|
|
740
|
+
now = time.monotonic()
|
|
741
|
+
if bus is None or not js.fresh(now, self._cfg.feedback_stale_s):
|
|
742
|
+
return
|
|
743
|
+
with self._lock:
|
|
744
|
+
pos = js.pos
|
|
745
|
+
js.target = pos
|
|
746
|
+
js.vel_target = 0.0
|
|
747
|
+
if dm_mode == dm.MODE_POS_VEL:
|
|
748
|
+
f = CanFrame(dm.ID_POS_VEL + js.esc_id, dm.pos_vel_frame(pos, js.vlim))
|
|
749
|
+
elif dm_mode == dm.MODE_MIT:
|
|
750
|
+
f = CanFrame(dm.ID_MIT + js.esc_id,
|
|
751
|
+
dm.mit_frame(pos, 0.0, js.kp, js.kd, 0.0, dm.R1_JOINT_LIMITS[js.index]))
|
|
752
|
+
elif dm_mode == dm.MODE_VEL:
|
|
753
|
+
f = CanFrame(dm.ID_VEL + js.esc_id, dm.vel_frame(0.0))
|
|
754
|
+
else:
|
|
755
|
+
return
|
|
756
|
+
try:
|
|
757
|
+
bus.send(f)
|
|
758
|
+
except Exception: # noqa: BLE001
|
|
759
|
+
pass
|
|
760
|
+
|
|
761
|
+
def _poke_feedback(self, force: bool = False) -> None:
|
|
762
|
+
"""When disabled and the feedback is stale, use 0x7FF 0xCC to make each of the six motors send one feedback.
|
|
763
|
+
Rate-limited; never takes over by accident."""
|
|
764
|
+
now = time.monotonic()
|
|
765
|
+
if not force and (now - self._last_poke_t) < POKE_MIN_GAP_S:
|
|
766
|
+
return
|
|
767
|
+
if self._blocked or self._bus is None:
|
|
768
|
+
return
|
|
769
|
+
self._last_poke_t = now
|
|
770
|
+
try:
|
|
771
|
+
self._bus.send_many([CanFrame(dm.ID_REG, dm.reg_refresh_frame(j.esc_id)) for j in self._joints])
|
|
772
|
+
except Exception: # noqa: BLE001
|
|
773
|
+
pass
|
|
774
|
+
|
|
775
|
+
def _hold_frames(self) -> List[CanFrame]:
|
|
776
|
+
now = time.monotonic()
|
|
777
|
+
out: List[CanFrame] = []
|
|
778
|
+
with self._lock:
|
|
779
|
+
if not self._targets_valid:
|
|
780
|
+
return out
|
|
781
|
+
if self._mode == ArmMode.POSITION:
|
|
782
|
+
for j in self._joints:
|
|
783
|
+
out.append(CanFrame(dm.ID_POS_VEL + j.esc_id, dm.pos_vel_frame(j.target, j.vlim)))
|
|
784
|
+
elif self._mode == ArmMode.SERVO:
|
|
785
|
+
for j in self._joints:
|
|
786
|
+
out.append(CanFrame(dm.ID_MIT + j.esc_id,
|
|
787
|
+
dm.mit_frame(j.target, 0.0, j.kp, j.kd, 0.0, dm.R1_JOINT_LIMITS[j.index])))
|
|
788
|
+
elif self._mode == ArmMode.VELOCITY:
|
|
789
|
+
expired = self._vel_until and now >= self._vel_until
|
|
790
|
+
for j in self._joints:
|
|
791
|
+
v = 0.0 if expired else j.vel_target
|
|
792
|
+
out.append(CanFrame(dm.ID_VEL + j.esc_id, dm.vel_frame(v)))
|
|
793
|
+
return out
|
|
794
|
+
|
|
795
|
+
def _send_hold_once(self) -> None:
|
|
796
|
+
bus = self._bus
|
|
797
|
+
frames = self._hold_frames()
|
|
798
|
+
if bus is not None and frames:
|
|
799
|
+
try:
|
|
800
|
+
bus.send_many(frames)
|
|
801
|
+
except Exception: # noqa: BLE001
|
|
802
|
+
pass
|
|
803
|
+
|
|
804
|
+
def _keepalive_loop(self) -> None:
|
|
805
|
+
period = self._cfg.keepalive_s
|
|
806
|
+
next_t = time.monotonic()
|
|
807
|
+
while not self._ka_stop.is_set():
|
|
808
|
+
next_t += period
|
|
809
|
+
dt = next_t - time.monotonic()
|
|
810
|
+
if dt > 0:
|
|
811
|
+
self._ka_stop.wait(dt)
|
|
812
|
+
else:
|
|
813
|
+
next_t = time.monotonic()
|
|
814
|
+
if self._ka_stop.is_set():
|
|
815
|
+
break
|
|
816
|
+
if self._enabled:
|
|
817
|
+
self._send_hold_once()
|
|
818
|
+
self._poll_changes()
|
|
819
|
+
|
|
820
|
+
def _poll_changes(self) -> None:
|
|
821
|
+
st = self._compute_state()
|
|
822
|
+
if st != self._last_state:
|
|
823
|
+
self._last_state = st
|
|
824
|
+
self._fire("state", {"state": st})
|
|
825
|
+
ew = self._compute_err_warn()
|
|
826
|
+
if ew != self._last_err_warn:
|
|
827
|
+
self._last_err_warn = ew
|
|
828
|
+
self._fire("error_warn", {"error_code": ew[0], "warn_code": ew[1]})
|
|
829
|
+
|
|
830
|
+
# =================================================================== internal: target management
|
|
831
|
+
def _reseed_targets(self) -> None:
|
|
832
|
+
now = time.monotonic()
|
|
833
|
+
with self._lock:
|
|
834
|
+
for j in self._joints:
|
|
835
|
+
if j.fresh(now, 1.0):
|
|
836
|
+
j.target = j.pos
|
|
837
|
+
j.vel_target = 0.0
|
|
838
|
+
self._targets_valid = all(j.fresh(now, 1.0) for j in self._joints)
|
|
839
|
+
|
|
840
|
+
def _hold_here(self) -> None:
|
|
841
|
+
with self._lock:
|
|
842
|
+
for j in self._joints:
|
|
843
|
+
if j.fresh(time.monotonic(), 1.0):
|
|
844
|
+
j.target = j.pos
|
|
845
|
+
j.vel_target = 0.0
|
|
846
|
+
self._vel_until = 0.0
|
|
847
|
+
self._send_hold_once()
|
|
848
|
+
|
|
849
|
+
# =================================================================== internal: receive callbacks
|
|
850
|
+
def _on_bus(self, cmd: int, frame: Optional[CanFrame], now: float) -> None:
|
|
851
|
+
if cmd == -1:
|
|
852
|
+
# serial port died: keep-alive sends will fail and the state callback will report UNKNOWN
|
|
853
|
+
return
|
|
854
|
+
if cmd == REP_HEARTBEAT:
|
|
855
|
+
self._heartbeat_t = now
|
|
856
|
+
return
|
|
857
|
+
if frame is None:
|
|
858
|
+
return
|
|
859
|
+
if is_local_report(cmd):
|
|
860
|
+
self._on_local(cmd, frame.data, now)
|
|
861
|
+
return
|
|
862
|
+
if cmd == REP_RX_OK:
|
|
863
|
+
self._on_can_rx(frame, now)
|
|
864
|
+
return
|
|
865
|
+
if cmd in (REP_TX_OK, REP_TX_FAIL):
|
|
866
|
+
with self._cv:
|
|
867
|
+
self._tx_results[frame.id] = (cmd, now)
|
|
868
|
+
if self._is_arm_frame(frame):
|
|
869
|
+
if cmd == REP_TX_FAIL:
|
|
870
|
+
self._tx_fail_streak += 1
|
|
871
|
+
if self._tx_fail_streak >= BLOCKED_STREAK and not self._blocked:
|
|
872
|
+
self._blocked = True
|
|
873
|
+
self._targets_valid = False
|
|
874
|
+
else:
|
|
875
|
+
self._tx_fail_streak = 0
|
|
876
|
+
if self._blocked:
|
|
877
|
+
self._blocked = False
|
|
878
|
+
self._cv.notify_all()
|
|
879
|
+
return
|
|
880
|
+
|
|
881
|
+
def _on_local(self, cmd: int, p: bytes, now: float) -> None:
|
|
882
|
+
"""Board local reports (USB-CAN bridge 0x20~0x25); merged with the log port $S/$E into the same place."""
|
|
883
|
+
if cmd == REP_LOCAL_STATUS:
|
|
884
|
+
st = pr.decode_local_status(p, now, self._board)
|
|
885
|
+
if st is not None:
|
|
886
|
+
self._on_board_status(st)
|
|
887
|
+
elif cmd == REP_LOCAL_EVENT:
|
|
888
|
+
ev = pr.decode_local_event(p, now)
|
|
889
|
+
if ev is not None:
|
|
890
|
+
self._on_board_event(ev)
|
|
891
|
+
elif cmd == REP_LOCAL_VERSION:
|
|
892
|
+
v = pr.decode_local_version(p)
|
|
893
|
+
if v is not None:
|
|
894
|
+
with self._cv:
|
|
895
|
+
self._board_version = v
|
|
896
|
+
self._cv.notify_all()
|
|
897
|
+
elif cmd in (REP_LOCAL_POS, REP_LOCAL_TAU, REP_LOCAL_TAUG):
|
|
898
|
+
b = self._board
|
|
899
|
+
if b is None or len(p) < 2 * NJ:
|
|
900
|
+
return
|
|
901
|
+
if cmd == REP_LOCAL_POS:
|
|
902
|
+
b.pos = pr.decode_local_pos(p, [lim.pmax for lim in dm.R1_JOINT_LIMITS])
|
|
903
|
+
elif cmd == REP_LOCAL_TAU:
|
|
904
|
+
b.tau = pr.decode_local_i16x6(p)
|
|
905
|
+
else:
|
|
906
|
+
b.tau_g = pr.decode_local_i16x6(p)
|
|
907
|
+
|
|
908
|
+
def _on_can_rx(self, f: CanFrame, now: float) -> None:
|
|
909
|
+
if f.ide:
|
|
910
|
+
return
|
|
911
|
+
esc_lo, esc_hi = min(self._cfg.esc_ids), max(self._cfg.esc_ids)
|
|
912
|
+
if f.id == dm.ID_REG or dm.looks_like_reg_reply(f.data, esc_lo, esc_hi):
|
|
913
|
+
rep = dm.decode_reg_reply(f.data)
|
|
914
|
+
if rep is not None:
|
|
915
|
+
with self._cv:
|
|
916
|
+
self._reg_replies[(rep.esc_id, rep.rid, rep.op)] = rep
|
|
917
|
+
self._cv.notify_all()
|
|
918
|
+
return
|
|
919
|
+
if f.id == pr.CAN_ID_GRIP_ST:
|
|
920
|
+
g = pr.decode_grip_status(f.data)
|
|
921
|
+
if g is not None:
|
|
922
|
+
self._gripper = g
|
|
923
|
+
self._gripper_t = now
|
|
924
|
+
return
|
|
925
|
+
if f.id in (pr.CAN_ID_BTN, pr.CAN_ID_UI, pr.CAN_ID_GRIP_CMD):
|
|
926
|
+
return
|
|
927
|
+
if len(f.data) < 6:
|
|
928
|
+
return
|
|
929
|
+
esc = f.data[0] & 0x0F
|
|
930
|
+
j = self._joint_by_esc(esc)
|
|
931
|
+
if j is None:
|
|
932
|
+
return
|
|
933
|
+
# Must also check CAN ID == MST_ID (0x10 + motor number; 0x00 is the Damiao factory default, kept as fallback).
|
|
934
|
+
# The board forwards every frame on the bus; in the MIT frames the end board sends to the gripper motor
|
|
935
|
+
# (ESC 0x07), data[0] is the high byte of the target position, whose low nibble collides with joints 1~6 with
|
|
936
|
+
# probability 6/16 -- this once overwrote J3 with -187 deg / status 0x4 / a fake fault.
|
|
937
|
+
# The bottom board r1_arm.c and the end board r1_gripper_dm.c filter the same way.
|
|
938
|
+
if f.id != pr.JOINT_MST_BASE + esc and f.id != 0:
|
|
939
|
+
return
|
|
940
|
+
fb = dm.decode_feedback(f.data, dm.R1_JOINT_LIMITS[j])
|
|
941
|
+
if fb is None or abs(fb.pos) > POS_SANE_RAD:
|
|
942
|
+
return
|
|
943
|
+
with self._cv:
|
|
944
|
+
js = self._joints[j]
|
|
945
|
+
js.pos, js.vel, js.tau = fb.pos, fb.vel, fb.tau
|
|
946
|
+
js.status, js.t_mos, js.t_rotor = fb.status, fb.t_mos, fb.t_rotor
|
|
947
|
+
js.t = now
|
|
948
|
+
snap = JointState(**vars(js))
|
|
949
|
+
self._cv.notify_all()
|
|
950
|
+
self._fire("feedback", snap)
|
|
951
|
+
|
|
952
|
+
def _on_log(self, item) -> None:
|
|
953
|
+
if isinstance(item, pr.BoardStatus):
|
|
954
|
+
self._on_board_status(item)
|
|
955
|
+
elif isinstance(item, pr.BoardEvent):
|
|
956
|
+
self._on_board_event(item)
|
|
957
|
+
|
|
958
|
+
def _on_board_status(self, item: pr.BoardStatus) -> None:
|
|
959
|
+
"""The board status is the authoritative source of "who is in control": the blocked flag sets _blocked
|
|
960
|
+
directly, no need to wait for enough TX_FAILs to accumulate."""
|
|
961
|
+
self._board = item
|
|
962
|
+
if item.state == pr.ST_ESTOP or item.flag(pr.SF_ESTOP):
|
|
963
|
+
self._estop_by_event = True
|
|
964
|
+
elif self._estop_by_event:
|
|
965
|
+
self._estop_by_event = False
|
|
966
|
+
if item.flag(pr.SF_HOST_BLOCKED):
|
|
967
|
+
if not self._blocked:
|
|
968
|
+
self._blocked = True
|
|
969
|
+
self._targets_valid = False
|
|
970
|
+
elif item.state == pr.ST_NORMAL and self._blocked:
|
|
971
|
+
self._blocked = False
|
|
972
|
+
self._tx_fail_streak = 0
|
|
973
|
+
|
|
974
|
+
def _on_board_event(self, item: pr.BoardEvent) -> None:
|
|
975
|
+
if item.code == pr.EV_ESTOP_ON:
|
|
976
|
+
self._estop_by_event = True
|
|
977
|
+
elif item.code == pr.EV_ESTOP_OFF:
|
|
978
|
+
self._estop_by_event = False
|
|
979
|
+
self._fire("event", item)
|
|
980
|
+
|
|
981
|
+
def _is_arm_frame(self, f: CanFrame) -> bool:
|
|
982
|
+
if f.ide:
|
|
983
|
+
return False
|
|
984
|
+
base = f.id & 0x700
|
|
985
|
+
if base not in (dm.ID_MIT, dm.ID_POS_VEL, dm.ID_VEL, dm.ID_FORCE_POS):
|
|
986
|
+
return False
|
|
987
|
+
return self._joint_by_esc(f.id & 0x0FF) is not None
|
|
988
|
+
|
|
989
|
+
# =================================================================== internal: misc
|
|
990
|
+
def _reset_runtime(self) -> None:
|
|
991
|
+
with self._lock:
|
|
992
|
+
for j in self._joints:
|
|
993
|
+
j.t = 0.0
|
|
994
|
+
j.dm_mode = 0
|
|
995
|
+
self._enabled = False
|
|
996
|
+
self._targets_valid = False
|
|
997
|
+
self._stopping_until = 0.0
|
|
998
|
+
self._blocked = False
|
|
999
|
+
self._tx_fail_streak = 0
|
|
1000
|
+
self._tx_results.clear()
|
|
1001
|
+
self._reg_replies.clear()
|
|
1002
|
+
self._gripper = None
|
|
1003
|
+
self._gripper_t = 0.0
|
|
1004
|
+
self._board = None
|
|
1005
|
+
self._board_version = None
|
|
1006
|
+
self._estop_by_event = None
|
|
1007
|
+
self._last_poke_t = 0.0
|
|
1008
|
+
self._last_state = ArmState.UNKNOWN
|
|
1009
|
+
self._last_err_warn = (R1Error.NONE, R1Warn.NONE)
|
|
1010
|
+
|
|
1011
|
+
def _joint_indices(self, servo_id: Optional[int]) -> Optional[List[int]]:
|
|
1012
|
+
if servo_id is None:
|
|
1013
|
+
return list(range(NJ))
|
|
1014
|
+
if isinstance(servo_id, int) and not isinstance(servo_id, bool) and 1 <= servo_id <= NJ:
|
|
1015
|
+
return [servo_id - 1]
|
|
1016
|
+
return None
|
|
1017
|
+
|
|
1018
|
+
def _joint_by_esc(self, esc: int) -> Optional[int]:
|
|
1019
|
+
for j in self._joints:
|
|
1020
|
+
if j.esc_id == esc:
|
|
1021
|
+
return j.index
|
|
1022
|
+
return None
|
|
1023
|
+
|
|
1024
|
+
def _speed_rad(self, speed, rad: bool) -> Optional[float]:
|
|
1025
|
+
if speed is None:
|
|
1026
|
+
return math.radians(self._cfg.speed_default)
|
|
1027
|
+
if not isinstance(speed, (int, float)) or speed <= 0:
|
|
1028
|
+
return None
|
|
1029
|
+
v = float(speed) if rad else math.radians(float(speed))
|
|
1030
|
+
return max(0.02, min(10.0, v))
|
|
1031
|
+
|
|
1032
|
+
def _wait_until(self, pred: Callable[[], bool], timeout: float) -> bool:
|
|
1033
|
+
deadline = time.monotonic() + timeout
|
|
1034
|
+
with self._cv:
|
|
1035
|
+
while not pred():
|
|
1036
|
+
left = deadline - time.monotonic()
|
|
1037
|
+
if left <= 0:
|
|
1038
|
+
return False
|
|
1039
|
+
self._cv.wait(min(left, 0.05))
|
|
1040
|
+
return True
|
|
1041
|
+
|
|
1042
|
+
def _set_last_error(self, msg: str) -> None:
|
|
1043
|
+
self._last_error = msg
|
|
1044
|
+
|
|
1045
|
+
def _reg(self, kind: str, cb) -> bool:
|
|
1046
|
+
if not callable(cb):
|
|
1047
|
+
return False
|
|
1048
|
+
if cb not in self._cb[kind]:
|
|
1049
|
+
self._cb[kind].append(cb)
|
|
1050
|
+
return True
|
|
1051
|
+
|
|
1052
|
+
def _unreg(self, kind: str, cb) -> bool:
|
|
1053
|
+
if cb is None:
|
|
1054
|
+
self._cb[kind].clear()
|
|
1055
|
+
return True
|
|
1056
|
+
if cb in self._cb[kind]:
|
|
1057
|
+
self._cb[kind].remove(cb)
|
|
1058
|
+
return True
|
|
1059
|
+
return False
|
|
1060
|
+
|
|
1061
|
+
def _fire(self, kind: str, payload) -> None:
|
|
1062
|
+
for cb in list(self._cb.get(kind, ())):
|
|
1063
|
+
try:
|
|
1064
|
+
cb(payload)
|
|
1065
|
+
except Exception: # noqa: BLE001
|
|
1066
|
+
pass
|
|
1067
|
+
|
|
1068
|
+
# =================================================================== with
|
|
1069
|
+
def __enter__(self):
|
|
1070
|
+
if not self.connected:
|
|
1071
|
+
self.connect()
|
|
1072
|
+
return self
|
|
1073
|
+
|
|
1074
|
+
def __exit__(self, exc_type, exc, tb) -> None:
|
|
1075
|
+
self.disconnect()
|