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

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
@@ -0,0 +1,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