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/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
|
r1_python_sdk/config.py
ADDED
|
@@ -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]
|
r1_python_sdk/damiao.py
ADDED
|
@@ -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
|