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/board.py ADDED
@@ -0,0 +1,71 @@
1
+ """Board: bottom-board state machine, end-effector buttons, taught-trajectory info (read-only).
2
+
3
+ Data comes from the board's local reports over the USB-CAN bridge (0x20 STATUS every 40 ms, 0x22/0x23/0x24
4
+ positions / torques, 0x21 events), so no second cable is needed; if the USART2 log port is connected its ``$S/$E``
5
+ lines feed the same BoardStatus. If none arrives within 0.5 s the APIs return NO_BOARD_REPORT (board firmware too old,
6
+ or the board has not reached NORMAL yet).
7
+
8
+ The two end-effector buttons (blue = teach / green = replay) and the board's E-stop mushroom are handled by the board
9
+ itself; the PC can only "see" them: user presses blue -> get_state() becomes TEACHING and PC commands return
10
+ HOST_BLOCKED; release -> PAUSED (board HOLD); after an E-stop reset also PAUSED; only motion_enable(True) takes control
11
+ back. Teach / replay have no PC-triggered API on purpose: R1 is designed to be taught by a person at the arm with
12
+ the end-effector buttons.
13
+ """
14
+
15
+ from __future__ import annotations
16
+
17
+ from typing import Tuple
18
+
19
+ from . import protocol as pr
20
+ from .code import R1State
21
+ from .decorator import r1_connected
22
+ from .base import Base
23
+
24
+
25
+ class Board(Base):
26
+
27
+ @r1_connected("get")
28
+ def get_board_state(self) -> Tuple[int, pr.BoardStatus]:
29
+ """(code, BoardStatus): board state machine, flags, per-axis feedback bitmap, trajectory point count / progress,
30
+ motor mode / status nibbles, joint positions, measured torques, gravity feed-forward torques."""
31
+ b = self._board_fresh()
32
+ if b is None:
33
+ return R1State.NO_BOARD_REPORT, None
34
+ return R1State.NORMAL, b
35
+
36
+ @r1_connected("get")
37
+ def get_end_button_state(self) -> Tuple[int, dict]:
38
+ """(code, {'blue': bool, 'green': bool, 'alive': bool, 'estop': bool}).
39
+ blue: blue button held (teach); green: green button held; alive: end-effector button heartbeat present;
40
+ estop: board E-stop mushroom pressed."""
41
+ b = self._board_fresh()
42
+ if b is None:
43
+ return R1State.NO_BOARD_REPORT, None
44
+ return R1State.NORMAL, {
45
+ "blue": b.flag(pr.SF_BLUE),
46
+ "green": b.flag(pr.SF_GREEN),
47
+ "alive": b.flag(pr.SF_BTN_ALIVE),
48
+ "estop": b.flag(pr.SF_ESTOP) or b.state == pr.ST_ESTOP,
49
+ }
50
+
51
+ @r1_connected("get")
52
+ def get_trajectory_info(self) -> Tuple[int, dict]:
53
+ """(code, {'points': n, 'duration': s, 'progress': i, 'direction': 'B'|'A', 'recording': bool, 'playing': bool}).
54
+ The taught trajectory stored in the board: point count (x 40 ms = duration), replay progress, direction,
55
+ whether it is currently recording / playing."""
56
+ b = self._board_fresh()
57
+ if b is None:
58
+ return R1State.NO_BOARD_REPORT, None
59
+ return R1State.NORMAL, {
60
+ "points": b.traj_n,
61
+ "duration": b.traj_n * pr.REC_PERIOD_MS / 1000.0,
62
+ "progress": b.progress,
63
+ "direction": "B" if b.flag(pr.SF_PLAY_FWD) else "A",
64
+ "recording": b.state == pr.ST_TEACH,
65
+ "playing": b.state == pr.ST_REPLAY,
66
+ }
67
+
68
+ @property
69
+ def has_board_report(self) -> bool:
70
+ """A board status was received within the last 0.5 s."""
71
+ return self._board_fresh() is not None
r1_python_sdk/code.py ADDED
@@ -0,0 +1,161 @@
1
+ """Return codes and error codes.
2
+
3
+ Conventions:
4
+ - set APIs return ``code``; get APIs return ``(code, value)``, and value is only valid when ``code == 0``.
5
+ - Negative: decided locally by the SDK (not connected, not enabled, bad parameter, feature missing...);
6
+ no command was sent.
7
+ - Positive: a device / communication side problem.
8
+
9
+ Three kinds of "not available" to keep apart:
10
+ - CMD_NOT_EXIST the R1 machine simply has no such feature (e.g. gripper enable is managed by the end board)
11
+ - NOT_IMPLEMENTED not written in the SDK yet (placeholder)
12
+ - FIRMWARE_PENDING the SDK side is ready, waiting for the firmware to expose the frame (firmware version query)
13
+ """
14
+
15
+ from __future__ import annotations
16
+
17
+
18
+ class R1State:
19
+ """API return codes."""
20
+
21
+ NORMAL = 0
22
+
23
+ # ---- negative: SDK local ----
24
+ NOT_CONNECTED = -1 # not connected
25
+ NOT_READY = -2 # not enabled, or board not in NORMAL (local control / E-stop / stopping)
26
+ API_EXCEPTION = -3 # exception inside the API, see last_error
27
+ CMD_NOT_EXIST = -4 # R1 has no such feature
28
+ NOT_IMPLEMENTED = -5 # not implemented in the SDK yet (placeholder)
29
+ FIRMWARE_PENDING = -6 # waiting for the firmware to expose the frame (placeholder)
30
+ JOINT_LIMIT = -7 # joint angle outside the soft limits
31
+ OUT_OF_RANGE = -8 # parameter out of range
32
+ EMERGENCY_STOP = -9 # board E-stop active
33
+ SERVO_NOT_EXIST = -10 # no such joint number
34
+ PARAM_ERROR = -11 # wrong parameter type / count
35
+ IK_NO_SOLUTION = -12 # inverse kinematics did not converge / target unreachable (Cartesian APIs)
36
+
37
+ # ---- positive: device / communication ----
38
+ HAS_ERROR = 1 # uncleared motor fault, see get_err_warn_code
39
+ HAS_WARN = 2
40
+ RES_TIMEOUT = 3 # timeout waiting for a reply (feedback / register reply)
41
+ TX_FAIL = 4 # board reports CAN send failure (bus not connected, no ACK)
42
+ HOST_BLOCKED = 20 # board in TEACH / REPLAY / HOLD, the PC's motion frames are blocked
43
+ FB_LOST = 21 # a joint has had no feedback for a long time
44
+ MODE_MISMATCH = 22 # actual motor control mode differs from set_mode
45
+ NO_BOARD_REPORT = 23 # no board status in 0.5 s (local report / $S): firmware too old or board not up
46
+ MOTOR_FAULT = 50 # motor fault (over/under-voltage, over-current, over-temp, comm lost, overload)
47
+ GRIPPER_FAULT = 60
48
+ GRIPPER_NOT_INIT = 61 # gripper not homed yet
49
+ GRIPPER_NO_FEEDBACK = 62 # no 0x7F2 from the end board
50
+ WAIT_FINISH_TIMEOUT = 100 # wait=True timed out waiting to reach the target
51
+
52
+ _TEXT = {
53
+ 0: "OK",
54
+ -1: "not connected",
55
+ -2: "not ready (not enabled / board not under PC control / stopping)",
56
+ -3: "API exception",
57
+ -4: "not supported by the R1",
58
+ -5: "not implemented in the SDK yet",
59
+ -6: "waiting for firmware support",
60
+ -7: "joint angle outside the soft limits",
61
+ -8: "parameter out of range",
62
+ -9: "E-stop active",
63
+ -10: "no such joint",
64
+ -11: "parameter error",
65
+ -12: "no IK solution / target unreachable",
66
+ 1: "uncleared motor fault",
67
+ 2: "warning present",
68
+ 3: "reply timeout",
69
+ 4: "CAN send failed",
70
+ 20: "board in local control, PC blocked",
71
+ 21: "feedback lost",
72
+ 22: "control mode mismatch",
73
+ 23: "no board status",
74
+ 50: "motor fault",
75
+ 60: "gripper fault",
76
+ 61: "gripper not homed",
77
+ 62: "no gripper feedback",
78
+ 100: "timed out waiting to finish",
79
+ }
80
+
81
+ @classmethod
82
+ def text(cls, code: int) -> str:
83
+ return cls._TEXT.get(code, f"unknown return code {code}")
84
+
85
+
86
+ class ArmState:
87
+ """Overall arm state returned by ``get_state()``."""
88
+
89
+ UNKNOWN = -1
90
+ READY = 0 # PC control, at rest
91
+ MOVING = 1
92
+ PAUSED = 3 # board HOLD (pinned after the blue button is released)
93
+ STOPPING = 4 # emergency_stop just triggered, pinning in progress
94
+ TEACHING = 5 # board TEACH (blue button held)
95
+ PLAYBACK = 6 # board REPLAY (green button)
96
+ ESTOP = 7 # E-stop pressed
97
+ INITIALIZING = 8 # board BOOT / PROBE / BRINGUP
98
+ BLOCKED = 9 # PC blocked (consecutive TX_FAIL) but no board status to tell HOLD/TEACH/REPLAY apart
99
+
100
+ _TEXT = {
101
+ -1: "unknown", 0: "ready", 1: "moving", 3: "board hold", 4: "stopping",
102
+ 5: "teaching", 6: "replaying", 7: "E-stop", 8: "initializing", 9: "board local control",
103
+ }
104
+
105
+ @classmethod
106
+ def text(cls, st: int) -> str:
107
+ return cls._TEXT.get(st, f"state {st}")
108
+
109
+
110
+ class ArmMode:
111
+ """Modes for ``set_mode()``."""
112
+
113
+ POSITION = 0 # Damiao position-velocity mode, one frame per move, motor plans internally (set_servo_angle)
114
+ SERVO = 1 # Damiao MIT, one frame per cycle (set_servo_angle_j / start_control_loop)
115
+ TEACH = 2 # board teach state (entered with the end-effector blue button); PC cannot set it: set_mode(2) returns CMD_NOT_EXIST
116
+ VELOCITY = 4 # Damiao velocity mode (vc_set_joint_velocity)
117
+
118
+ _TEXT = {0: "position", 1: "servo (MIT)", 2: "teach", 4: "velocity"}
119
+
120
+ @classmethod
121
+ def text(cls, m: int) -> str:
122
+ return cls._TEXT.get(m, f"mode {m}")
123
+
124
+
125
+ class R1Error:
126
+ """The err field returned by ``get_err_warn_code()``."""
127
+
128
+ NONE = 0
129
+ ESTOP = 1
130
+ FB_LOST = 2
131
+ BOARD_ALARM = 3
132
+ # motor fault: MOTOR_BASE + joint*16 + Damiao status nibble
133
+ # (8 over-voltage, 9 under-voltage, A over-current, B MOS over-temp, C coil over-temp, D comm lost, E overload)
134
+ MOTOR_BASE = 0x100
135
+
136
+ @staticmethod
137
+ def motor(joint: int, status: int) -> int:
138
+ return R1Error.MOTOR_BASE + joint * 16 + (status & 0x0F)
139
+
140
+ @staticmethod
141
+ def is_motor(err: int) -> bool:
142
+ return err >= R1Error.MOTOR_BASE
143
+
144
+ @staticmethod
145
+ def motor_joint(err: int) -> int:
146
+ return (err - R1Error.MOTOR_BASE) // 16
147
+
148
+ @staticmethod
149
+ def motor_status(err: int) -> int:
150
+ return (err - R1Error.MOTOR_BASE) % 16
151
+
152
+
153
+ class R1Warn:
154
+ """The warn field returned by ``get_err_warn_code()``."""
155
+
156
+ NONE = 0
157
+ HOST_BLOCKED = 1
158
+ FEEDBACK_STALE = 2
159
+ GRIPPER_FAULT = 3
160
+ GRIPPER_NOT_INIT = 4
161
+ MODE_MISMATCH = 5
@@ -0,0 +1,124 @@
1
+ """R1Config: runtime parameters such as per-joint gains, default speed, soft limits and keep-alive rate.
2
+
3
+ Parameters can live in a config file, but there is a usable set of defaults so no file is required::
4
+
5
+ arm = R1API() # defaults
6
+ arm = R1API(config=R1Config(kd=[3]*6)) # override in code
7
+ arm = R1API(config="my_r1.yaml") # YAML (needs pip install pyyaml)
8
+
9
+ YAML example::
10
+
11
+ kp: [120, 120, 120, 18, 18, 18]
12
+ kd: [5, 5, 5, 2, 2, 2]
13
+ speed_default: 30 # deg/s
14
+ joint_limits: # degrees, optional
15
+ - [-170, 170]
16
+ - [-120, 120]
17
+ - [-150, 150]
18
+ - [-170, 170]
19
+ - [-110, 110]
20
+ - [-360, 360]
21
+ keepalive_hz: 50
22
+
23
+ Angles in the config are always degrees (the file is for humans); ``is_radian`` only affects API arguments/returns.
24
+ """
25
+
26
+ from __future__ import annotations
27
+
28
+ from dataclasses import dataclass, field, fields
29
+ from pathlib import Path
30
+ from typing import List, Optional, Sequence, Tuple, Union
31
+
32
+ from . import damiao as dm
33
+ from .protocol import NJ
34
+
35
+ ConfigSource = Union[None, "R1Config", str, Path, dict]
36
+
37
+
38
+ @dataclass
39
+ class R1Config:
40
+ # MIT (servo mode) gains; kd encoding caps at 5 (damiao.KD_MAX)
41
+ kp: Sequence[float] = (120.0, 120.0, 120.0, 18.0, 18.0, 18.0)
42
+ kd: Sequence[float] = (5.0, 5.0, 5.0, 2.0, 2.0, 2.0)
43
+ # speed cap used by set_servo_angle in position mode when no speed is given, deg/s
44
+ speed_default: float = 30.0
45
+ # soft limits [(lo, hi)] x 6, degrees; None = unlimited
46
+ joint_limits: Optional[Sequence[Tuple[float, float]]] = None
47
+ # keep-alive / send period. In servo mode the user target is forwarded to the motors at this rate;
48
+ # it also feeds the Damiao CAN TIMEOUT watchdog
49
+ keepalive_hz: float = 50.0
50
+ # timeout waiting for a register reply / enable feedback, seconds
51
+ timeout: float = 1.0
52
+ # feedback older than this counts as stale (get_servo_angle returns FB_LOST)
53
+ feedback_stale_s: float = 0.5
54
+ # joint index -> CAN ID; default joint1 = 0x01 ... joint6 = 0x06
55
+ esc_ids: Sequence[int] = field(default_factory=lambda: tuple(range(1, NJ + 1)))
56
+
57
+ # ------------------------------------------------------------------ construction
58
+ @classmethod
59
+ def load(cls, src: ConfigSource) -> "R1Config":
60
+ if src is None:
61
+ cfg = cls()
62
+ elif isinstance(src, R1Config):
63
+ cfg = src
64
+ elif isinstance(src, dict):
65
+ cfg = cls.from_dict(src)
66
+ elif isinstance(src, (str, Path)):
67
+ cfg = cls.from_yaml(src)
68
+ else:
69
+ raise TypeError(f"config: unsupported type {type(src).__name__}")
70
+ cfg.validate()
71
+ return cfg
72
+
73
+ @classmethod
74
+ def from_dict(cls, d: dict) -> "R1Config":
75
+ known = {f.name for f in fields(cls)}
76
+ bad = set(d) - known
77
+ if bad:
78
+ raise ValueError(f"unknown config keys: {sorted(bad)}")
79
+ kw = dict(d)
80
+ if "joint_limits" in kw and kw["joint_limits"] is not None:
81
+ kw["joint_limits"] = [tuple(x) for x in kw["joint_limits"]]
82
+ return cls(**kw)
83
+
84
+ @classmethod
85
+ def from_yaml(cls, path: Union[str, Path]) -> "R1Config":
86
+ try:
87
+ import yaml # type: ignore
88
+ except ImportError as exc: # pragma: no cover
89
+ raise ImportError("reading a YAML config needs pyyaml: pip install r1-python-sdk[yaml]") from exc
90
+ data = yaml.safe_load(Path(path).read_text(encoding="utf-8")) or {}
91
+ if not isinstance(data, dict):
92
+ raise ValueError(f"{path}: the top level must be a mapping")
93
+ return cls.from_dict(data)
94
+
95
+ # ------------------------------------------------------------------ validation
96
+ def validate(self) -> None:
97
+ if len(self.kp) != NJ or len(self.kd) != NJ:
98
+ raise ValueError(f"kp and kd must each have {NJ} values")
99
+ if any(not (0 <= float(k) <= dm.KP_MAX) for k in self.kp):
100
+ raise ValueError(f"kp out of range 0~{dm.KP_MAX}")
101
+ if any(not (0 <= float(k) <= dm.KD_MAX) for k in self.kd):
102
+ raise ValueError(f"kd out of range 0~{dm.KD_MAX} (Damiao encoding limit)")
103
+ if not (0 < float(self.speed_default) <= 600):
104
+ raise ValueError("speed_default must be within 0~600 deg/s")
105
+ if self.joint_limits is not None:
106
+ if len(self.joint_limits) != NJ or any(lo >= hi for lo, hi in self.joint_limits):
107
+ raise ValueError("joint_limits needs 6 (lo, hi) pairs with lo < hi")
108
+ if not (5 <= float(self.keepalive_hz) <= 500):
109
+ raise ValueError("keepalive_hz must be within 5~500")
110
+ if float(self.timeout) <= 0 or float(self.feedback_stale_s) <= 0:
111
+ raise ValueError("timeout / feedback_stale_s must be > 0")
112
+ if len(self.esc_ids) != NJ or len(set(self.esc_ids)) != NJ:
113
+ raise ValueError(f"esc_ids must be {NJ} distinct CAN IDs")
114
+
115
+ # ------------------------------------------------------------------ convenience
116
+ @property
117
+ def keepalive_s(self) -> float:
118
+ return 1.0 / float(self.keepalive_hz)
119
+
120
+ def kp_list(self) -> List[float]:
121
+ return [float(x) for x in self.kp]
122
+
123
+ def kd_list(self) -> List[float]:
124
+ return [float(x) for x in self.kd]
@@ -0,0 +1,239 @@
1
+ """Damiao motor CAN frame encode / decode (pure functions, no hardware access).
2
+
3
+ The ranges PMAX/VMAX/TMAX must match the registers inside the motor, otherwise positions are wrong.
4
+ R1's six joints: 1~3 DM4340P, 4~6 DM4310, matching the table in the bottom-board firmware r1_arm.c.
5
+ """
6
+
7
+ from __future__ import annotations
8
+
9
+ import struct
10
+ from dataclasses import dataclass
11
+ from typing import Optional
12
+
13
+ # ---- CAN ID offset (per mode) ----
14
+ ID_MIT = 0x000
15
+ ID_POS_VEL = 0x100
16
+ ID_VEL = 0x200
17
+ ID_FORCE_POS = 0x300
18
+ ID_REG = 0x7FF
19
+
20
+ # ---- control frame trailing byte FF FF FF FF FF FF FF xx ----
21
+ CTRL_ENABLE = 0xFC
22
+ CTRL_DISABLE = 0xFD
23
+ CTRL_SET_ZERO = 0xFE
24
+ CTRL_CLEAR_ERR = 0xFB
25
+
26
+ # ---- control mode (value of the CTRL_MODE register) ----
27
+ MODE_MIT = 1
28
+ MODE_POS_VEL = 2
29
+ MODE_VEL = 3
30
+ MODE_FORCE_POS = 4
31
+ MODE_NAMES = {MODE_MIT: "MIT", MODE_POS_VEL: "position-velocity", MODE_VEL: "velocity", MODE_FORCE_POS: "force-position"}
32
+
33
+ # ---- 0x7FF registers ----
34
+ REG_READ = 0x33
35
+ REG_WRITE = 0x55
36
+ REG_SAVE = 0xAA
37
+ REG_REFRESH = 0xCC
38
+
39
+ RID_UV = 0
40
+ RID_KT = 1
41
+ RID_OT = 2
42
+ RID_OC = 3
43
+ RID_ACC = 4
44
+ RID_DEC = 5
45
+ RID_MAX_SPD = 6
46
+ RID_MST_ID = 7
47
+ RID_ESC_ID = 8
48
+ RID_TIMEOUT = 9
49
+ RID_CTRL_MODE = 10
50
+ RID_HW_VER = 13
51
+ RID_SW_VER = 14
52
+ RID_SN = 15
53
+ RID_PMAX = 21
54
+ RID_VMAX = 22
55
+ RID_TMAX = 23
56
+ RID_SUB_VER = 36
57
+ RID_POS_PRECISE = 0x50
58
+
59
+ # Which RIDs are integers (the rest are float). From is_in_ranges in the official DM_CAN.py.
60
+ _U32_RIDS = set(range(7, 11)) | set(range(13, 17)) | {35, 36}
61
+
62
+
63
+ def rid_is_u32(rid: int) -> bool:
64
+ return rid in _U32_RIDS
65
+
66
+
67
+ # ---- feedback status nibble data[0] >> 4 ----
68
+ STATUS_DISABLED = 0x0
69
+ STATUS_ENABLED = 0x1
70
+ STATUS_NAMES = {
71
+ 0x0: "disabled", 0x1: "enabled",
72
+ 0x8: "over-voltage", 0x9: "under-voltage", 0xA: "over-current", 0xB: "MOS over-temperature",
73
+ 0xC: "coil over-temperature", 0xD: "communication lost", 0xE: "overload",
74
+ }
75
+
76
+
77
+ def status_is_fault(st: int) -> bool:
78
+ return st >= 0x8
79
+
80
+
81
+ @dataclass(frozen=True)
82
+ class MotorLimits:
83
+ pmax: float
84
+ vmax: float
85
+ tmax: float
86
+
87
+
88
+ LIMITS_4340P = MotorLimits(12.5, 10.0, 28.0)
89
+ LIMITS_4310 = MotorLimits(12.5, 30.0, 10.0)
90
+ R1_JOINT_LIMITS = (LIMITS_4340P,) * 3 + (LIMITS_4310,) * 3
91
+
92
+ KP_MAX = 500.0
93
+ KD_MAX = 5.0
94
+
95
+
96
+ def float_to_uint(x: float, xmin: float, xmax: float, bits: int) -> int:
97
+ if x < xmin:
98
+ x = xmin
99
+ elif x > xmax:
100
+ x = xmax
101
+ span = xmax - xmin
102
+ return int((x - xmin) * ((1 << bits) - 1) / span)
103
+
104
+
105
+ def uint_to_float(x: int, xmin: float, xmax: float, bits: int) -> float:
106
+ span = xmax - xmin
107
+ return x * span / ((1 << bits) - 1) + xmin
108
+
109
+
110
+ # ---------------------------------------------------------------- encode
111
+
112
+ def ctrl_frame(cmd: int) -> bytes:
113
+ return bytes([0xFF] * 7 + [cmd & 0xFF])
114
+
115
+
116
+ def mit_frame(pos: float, vel: float, kp: float, kd: float, tau: float, lim: MotorLimits) -> bytes:
117
+ p = float_to_uint(pos, -lim.pmax, lim.pmax, 16)
118
+ v = float_to_uint(vel, -lim.vmax, lim.vmax, 12)
119
+ kp_u = float_to_uint(kp, 0.0, KP_MAX, 12)
120
+ kd_u = float_to_uint(kd, 0.0, KD_MAX, 12)
121
+ t = float_to_uint(tau, -lim.tmax, lim.tmax, 12)
122
+ return bytes([
123
+ (p >> 8) & 0xFF, p & 0xFF,
124
+ (v >> 4) & 0xFF, ((v & 0x0F) << 4) | ((kp_u >> 8) & 0x0F),
125
+ kp_u & 0xFF,
126
+ (kd_u >> 4) & 0xFF, ((kd_u & 0x0F) << 4) | ((t >> 8) & 0x0F),
127
+ t & 0xFF,
128
+ ])
129
+
130
+
131
+ def pos_vel_frame(pos: float, vlim: float) -> bytes:
132
+ """Position-velocity mode: p_des target position in rad, v_des speed cap during the move in rad/s
133
+ (per the manual: a cap, not a target velocity)."""
134
+ return struct.pack("<ff", float(pos), float(vlim))
135
+
136
+
137
+ def vel_frame(vel: float) -> bytes:
138
+ return struct.pack("<f", float(vel))
139
+
140
+
141
+ def force_pos_frame(pos: float, vlim: float, i_ratio: float) -> bytes:
142
+ """Force-position hybrid: pos in rad, vlim in rad/s (x100 as u16, max 100), i_ratio 0~1 per-unit current (x10000 as u16)."""
143
+ v = max(0, min(10000, int(round(vlim * 100.0))))
144
+ i = max(0, min(10000, int(round(i_ratio * 10000.0))))
145
+ return struct.pack("<fHH", float(pos), v, i)
146
+
147
+
148
+ def reg_read_frame(esc_id: int, rid: int) -> bytes:
149
+ return bytes([esc_id & 0xFF, (esc_id >> 8) & 0xFF, REG_READ, rid & 0xFF, 0, 0, 0, 0])
150
+
151
+
152
+ def reg_write_frame(esc_id: int, rid: int, value) -> bytes:
153
+ if rid_is_u32(rid):
154
+ payload = struct.pack("<I", int(value) & 0xFFFFFFFF)
155
+ else:
156
+ payload = struct.pack("<f", float(value))
157
+ return bytes([esc_id & 0xFF, (esc_id >> 8) & 0xFF, REG_WRITE, rid & 0xFF]) + payload
158
+
159
+
160
+ def reg_save_frame(esc_id: int) -> bytes:
161
+ return bytes([esc_id & 0xFF, (esc_id >> 8) & 0xFF, REG_SAVE, 0, 0, 0, 0, 0])
162
+
163
+
164
+ def reg_refresh_frame(esc_id: int) -> bytes:
165
+ """0x7FF 0xCC: make the motor send one feedback frame without changing anything (refresh_motor_status in the
166
+ official DM_CAN.py). Not a register write, so the board does not treat it as a "PC takeover"; in HOLD state
167
+ it is simply rejected with TX_FAIL."""
168
+ return bytes([esc_id & 0xFF, (esc_id >> 8) & 0xFF, REG_REFRESH, 0, 0, 0, 0, 0])
169
+
170
+
171
+ # ---------------------------------------------------------------- decode
172
+
173
+ @dataclass
174
+ class Feedback:
175
+ esc_id: int
176
+ status: int
177
+ pos: float # rad
178
+ vel: float # rad/s
179
+ tau: float # Nm
180
+ t_mos: int # degC
181
+ t_rotor: int # degC
182
+
183
+ @property
184
+ def enabled(self) -> bool:
185
+ return self.status == STATUS_ENABLED
186
+
187
+ @property
188
+ def fault(self) -> bool:
189
+ return status_is_fault(self.status)
190
+
191
+
192
+ def decode_feedback(data: bytes, lim: MotorLimits) -> Optional[Feedback]:
193
+ """Feedback frame: [0]=status<<4|ID [1..2]=position [3..4.5]=velocity [4.5..5]=torque [6]=T_MOS [7]=T_Rotor."""
194
+ if len(data) < 6:
195
+ return None
196
+ esc = data[0] & 0x0F
197
+ st = data[0] >> 4
198
+ p_u = (data[1] << 8) | data[2]
199
+ v_u = (data[3] << 4) | (data[4] >> 4)
200
+ t_u = ((data[4] & 0x0F) << 8) | data[5]
201
+ return Feedback(
202
+ esc_id=esc,
203
+ status=st,
204
+ pos=uint_to_float(p_u, -lim.pmax, lim.pmax, 16),
205
+ vel=uint_to_float(v_u, -lim.vmax, lim.vmax, 12),
206
+ tau=uint_to_float(t_u, -lim.tmax, lim.tmax, 12),
207
+ t_mos=data[6] if len(data) > 6 else 0,
208
+ t_rotor=data[7] if len(data) > 7 else 0,
209
+ )
210
+
211
+
212
+ @dataclass
213
+ class RegReply:
214
+ esc_id: int
215
+ op: int # REG_READ / REG_WRITE
216
+ rid: int
217
+ raw: bytes # 4 bytes
218
+
219
+ @property
220
+ def value(self):
221
+ if rid_is_u32(self.rid):
222
+ return struct.unpack("<I", self.raw)[0]
223
+ return struct.unpack("<f", self.raw)[0]
224
+
225
+
226
+ def looks_like_reg_reply(data: bytes, esc_lo: int = 1, esc_hi: int = 6) -> bool:
227
+ """Recognise a register reply by content. In practice the motor sends the reply on MST_ID (0x10+id) rather than
228
+ 0x7FF, and it has the same length as a feedback frame, so the CAN ID alone is not enough (the bottom-board firmware
229
+ r1_arm.c does the same). A feedback frame with data[1]==0 and data[2] in {0x33,0x55} would mean pos ~ -12.49 rad,
230
+ which R1 cannot reach."""
231
+ return (len(data) >= 8 and data[1] == 0 and data[2] in (REG_READ, REG_WRITE)
232
+ and esc_lo <= data[0] <= esc_hi)
233
+
234
+
235
+ def decode_reg_reply(data: bytes) -> Optional[RegReply]:
236
+ """Motor reply to a 0x7FF read / write: [0..1] ESC ID, [2] 0x33/0x55, [3] RID, [4..7] value."""
237
+ if len(data) < 8 or data[2] not in (REG_READ, REG_WRITE):
238
+ return None
239
+ return RegReply(esc_id=data[0] | (data[1] << 8), op=data[2], rid=data[3], raw=bytes(data[4:8]))
@@ -0,0 +1,57 @@
1
+ """Entry-guard decorators: check connected / ready before an API runs and return a code if not.
2
+
3
+ For ``_type='set'`` APIs a failure returns the code; for ``'get'`` it returns ``(code, None)``.
4
+ """
5
+
6
+ from __future__ import annotations
7
+
8
+ import functools
9
+
10
+ from .code import R1State
11
+
12
+
13
+ def _fail(code: int, _type: str):
14
+ return code if _type == "set" else (code, None)
15
+
16
+
17
+ def r1_connected(_type: str = "set"):
18
+ def deco(func):
19
+ @functools.wraps(func)
20
+ def wrapper(self, *args, **kwargs):
21
+ if not self.connected:
22
+ self._set_last_error(f"{func.__name__}: not connected")
23
+ return _fail(R1State.NOT_CONNECTED, _type)
24
+ return func(self, *args, **kwargs)
25
+ return wrapper
26
+ return deco
27
+
28
+
29
+ def r1_ready(_type: str = "set"):
30
+ """Connected + enabled + the board lets the PC control (no E-stop, not taken over locally, not paused)."""
31
+ def deco(func):
32
+ @functools.wraps(func)
33
+ def wrapper(self, *args, **kwargs):
34
+ if not self.connected:
35
+ self._set_last_error(f"{func.__name__}: not connected")
36
+ return _fail(R1State.NOT_CONNECTED, _type)
37
+ code = self._ready_code()
38
+ if code != R1State.NORMAL:
39
+ self._set_last_error(f"{func.__name__}: {R1State.text(code)}")
40
+ return _fail(code, _type)
41
+ return func(self, *args, **kwargs)
42
+ return wrapper
43
+ return deco
44
+
45
+
46
+ def r1_catch(_type: str = "set"):
47
+ """Turn unexpected exceptions inside an API into API_EXCEPTION."""
48
+ def deco(func):
49
+ @functools.wraps(func)
50
+ def wrapper(self, *args, **kwargs):
51
+ try:
52
+ return func(self, *args, **kwargs)
53
+ except Exception as exc: # noqa: BLE001
54
+ self._set_last_error(f"{func.__name__}: {exc!r}")
55
+ return _fail(R1State.API_EXCEPTION, _type)
56
+ return wrapper
57
+ return deco