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,342 @@
1
+ """Kinematics: Cartesian-space APIs (PC-side FK / IK, numpy only).
2
+
3
+ R1 has no controller box, so FK/IK run on the PC. The arm geometry is embedded in ``r1_python_sdk.kin`` (transcribed
4
+ from ``R1_Arm_description/urdf/R1.urdf``), so neither the URDF nor a simulator is needed at run time.
5
+
6
+ Pose convention: ``[x, y, z, roll, pitch, yaw]``
7
+ - x/y/z in **mm**, base frame = ``base_link`` (mounting plane, +z up, +x forward);
8
+ - roll/pitch/yaw = URDF fixed-axis XYZ (R = Rz(yaw) Ry(pitch) Rx(roll)), in degrees unless ``is_radian``;
9
+ - the pose refers to the TCP: ``end_link`` (fingertip plane, +x along the gripper) plus ``set_tcp_offset``.
10
+
11
+ ``set_position`` moves along a straight Cartesian line (position lerp + SO(3) geodesic, minimum-jerk time scaling).
12
+ The whole path is solved with IK *before* anything is sent; if any waypoint is unreachable or would cross a joint
13
+ limit nothing moves and ``IK_NO_SOLUTION`` / ``JOINT_LIMIT`` is returned. The waypoints are then streamed to the
14
+ joint targets at ``config.keepalive_hz`` on the SDK control-loop thread (works in POSITION and SERVO mode).
15
+ """
16
+
17
+ from __future__ import annotations
18
+
19
+ import math
20
+ import threading
21
+ import time
22
+ from typing import List, Optional, Sequence, Tuple
23
+
24
+ import numpy as np
25
+
26
+ from .base import NJ, Base
27
+ from .code import ArmMode, R1State
28
+ from .decorator import r1_catch, r1_connected, r1_ready
29
+ from .kin import IKOptions, IKSolver, R1_CHAIN, Chain, T_to_pose, min_jerk, pose_to_T, rot_angle_between, rot_interp
30
+ from .utils import from_rad, is_number_seq, to_rad
31
+
32
+ REACH_TOL_RAD = math.radians(0.5)
33
+ REACH_VEL_RAD = 0.05
34
+
35
+ DEFAULT_LIN_SPEED_MM_S = 50.0
36
+ DEFAULT_ROT_SPEED_DEG_S = 60.0
37
+ MAX_LIN_SPEED_MM_S = 500.0
38
+ MAX_ROT_SPEED_RAD_S = math.radians(360.0)
39
+ MAX_STEP_RAD = 0.2 # a joint jump larger than this between two consecutive waypoints = branch flip, reject
40
+
41
+ # extra IK seeds tried after the reference angles (rad): elbow-up / elbow-down style postures of the R1
42
+ _FALLBACK_SEEDS = (
43
+ (0.0, -0.96, -1.12, -1.57, 0.94, 1.57),
44
+ (0.0, -1.2, -1.6, 0.0, 0.0, 0.0),
45
+ (0.0, -0.5, -0.5, 0.0, 0.0, 0.0),
46
+ (0.0, -2.0, -0.5, 0.0, 1.0, 0.0),
47
+ (0.0,) * 6,
48
+ )
49
+
50
+
51
+ class Kinematics(Base):
52
+
53
+ # ------------------------------------------------------------------ lazy state (mixin: no __init__)
54
+ def _kin(self) -> Tuple[Chain, IKSolver]:
55
+ if getattr(self, "_kin_solver", None) is None:
56
+ self._kin_chain: Chain = R1_CHAIN
57
+ self._kin_solver = IKSolver(self._kin_chain, IKOptions())
58
+ self._tcp_T: Optional[np.ndarray] = None
59
+ self._tcp_offset: List[float] = [0.0] * 6
60
+ self._cart_done = threading.Event()
61
+ self._cart_done.set()
62
+ self._cart_code = R1State.NORMAL
63
+ return self._kin_chain, self._kin_solver
64
+
65
+ @property
66
+ def kin_chain(self) -> Chain:
67
+ """The kinematic chain used by the Cartesian APIs (``r1_python_sdk.kin.Chain``)."""
68
+ return self._kin()[0]
69
+
70
+ # ------------------------------------------------------------------ TCP
71
+ def set_tcp_offset(self, offset, is_radian: Optional[bool] = None) -> int:
72
+ """Tool offset relative to end_link: [x, y, z, roll, pitch, yaw] (mm, angles per is_radian). None resets."""
73
+ self._kin()
74
+ if offset is None:
75
+ self._tcp_T = None
76
+ self._tcp_offset = [0.0] * 6
77
+ return R1State.NORMAL
78
+ if not is_number_seq(offset, 6):
79
+ return R1State.PARAM_ERROR
80
+ rad = self._is_radian if is_radian is None else bool(is_radian)
81
+ xyz = [float(v) / 1000.0 for v in offset[:3]]
82
+ rpy = [to_rad(float(v), rad) for v in offset[3:]]
83
+ self._tcp_T = pose_to_T(xyz, rpy)
84
+ self._tcp_offset = [float(v) for v in offset[:3]] + [from_rad(a, rad) for a in rpy]
85
+ return R1State.NORMAL
86
+
87
+ def get_tcp_offset(self, is_radian: Optional[bool] = None) -> Tuple[int, list]:
88
+ self._kin()
89
+ rad = self._is_radian if is_radian is None else bool(is_radian)
90
+ if self._tcp_T is None:
91
+ return R1State.NORMAL, [0.0] * 6
92
+ p, rpy = T_to_pose(self._tcp_T)
93
+ return R1State.NORMAL, [float(v) * 1000.0 for v in p] + [from_rad(a, rad) for a in rpy]
94
+
95
+ # ------------------------------------------------------------------ pure computation (works offline)
96
+ @r1_catch("get")
97
+ def get_forward_kinematics(self, angles, input_is_radian: Optional[bool] = None,
98
+ return_is_radian: Optional[bool] = None) -> Tuple[int, list]:
99
+ """(code, [x, y, z, roll, pitch, yaw]) of the TCP for the given 6 joint angles. No connection needed."""
100
+ chain, _ = self._kin()
101
+ if not is_number_seq(angles, NJ):
102
+ return R1State.PARAM_ERROR, None
103
+ rin = self._is_radian if input_is_radian is None else bool(input_is_radian)
104
+ rout = self._is_radian if return_is_radian is None else bool(return_is_radian)
105
+ q = [to_rad(float(a), rin) for a in angles]
106
+ return R1State.NORMAL, self._pose_from_T(chain.fk(q, self._tcp_T), rout)
107
+
108
+ @r1_catch("get")
109
+ def get_inverse_kinematics(self, pose, ref_angles=None, input_is_radian: Optional[bool] = None,
110
+ return_is_radian: Optional[bool] = None) -> Tuple[int, list]:
111
+ """(code, angles[6]) reaching ``pose`` = [x, y, z, roll, pitch, yaw]. ``ref_angles`` seeds the solver and
112
+ picks the branch (defaults to the current joint targets when connected, else the zero pose).
113
+ Joint soft limits (set_joint_limits / config) are respected. Returns IK_NO_SOLUTION when unreachable."""
114
+ chain, solver = self._kin()
115
+ if not is_number_seq(pose, 6):
116
+ return R1State.PARAM_ERROR, None
117
+ rin = self._is_radian if input_is_radian is None else bool(input_is_radian)
118
+ rout = self._is_radian if return_is_radian is None else bool(return_is_radian)
119
+ if ref_angles is not None:
120
+ if not is_number_seq(ref_angles, NJ):
121
+ return R1State.PARAM_ERROR, None
122
+ q_ref = np.array([to_rad(float(a), rin) for a in ref_angles])
123
+ else:
124
+ q_ref = self._current_q(prefer_target=True)
125
+ if q_ref is None:
126
+ q_ref = np.zeros(NJ)
127
+ T = self._T_from_pose(pose, rin)
128
+ lo, hi = self._ik_bounds()
129
+ res = solver.solve_multi_seed(T, [q_ref] + [np.array(s) for s in _FALLBACK_SEEDS], self._tcp_T, lo, hi)
130
+ if not res.success:
131
+ self._set_last_error(
132
+ f"get_inverse_kinematics: IK did not converge, residual {res.pos_err * 1000:.1f} mm / "
133
+ f"{math.degrees(res.rot_err):.1f} deg (joints at a limit: {[i + 1 for i in res.at_limit]})")
134
+ return R1State.IK_NO_SOLUTION, None
135
+ return R1State.NORMAL, [from_rad(float(v), rout) for v in res.q]
136
+
137
+ # ------------------------------------------------------------------ live
138
+ @r1_connected("get")
139
+ def get_position(self, is_radian: Optional[bool] = None) -> Tuple[int, list]:
140
+ """(code, [x, y, z, roll, pitch, yaw]) of the TCP from FK of the latest joint feedback.
141
+ Like get_servo_angle: stale feedback gives code=FB_LOST but the value is still returned."""
142
+ chain, _ = self._kin()
143
+ rad = self._is_radian if is_radian is None else bool(is_radian)
144
+ if not self._enabled and not self._joints_fresh():
145
+ self._poke_feedback()
146
+ with self._lock:
147
+ q = [j.pos for j in self._joints]
148
+ pose = self._pose_from_T(chain.fk(q, self._tcp_T), rad)
149
+ return (R1State.NORMAL if self._joints_fresh() else R1State.FB_LOST), pose
150
+
151
+ @r1_catch("set")
152
+ @r1_ready("set")
153
+ def set_position(self, x=None, y=None, z=None, roll=None, pitch=None, yaw=None,
154
+ speed: Optional[float] = None, wait: bool = False, timeout: Optional[float] = None,
155
+ relative: bool = False, is_radian: Optional[bool] = None,
156
+ rot_speed: Optional[float] = None) -> int:
157
+ """Straight-line Cartesian move of the TCP. Needs set_mode(0) or set_mode(1).
158
+ Components left as None keep their current value; with relative=True the given values are added to the
159
+ current pose (position in mm, angles added to the current roll/pitch/yaw).
160
+ speed: linear speed in mm/s (default 50); rot_speed: angular speed in deg/s or rad/s (default 60 deg/s);
161
+ the move takes max(dist/speed, angle/rot_speed) with a minimum-jerk profile.
162
+ The whole path is checked with IK first; on failure nothing moves and IK_NO_SOLUTION / JOINT_LIMIT is
163
+ returned. wait=True blocks until the arm has settled at the end point (or WAIT_FINISH_TIMEOUT)."""
164
+ chain, solver = self._kin()
165
+ if self._mode not in (ArmMode.POSITION, ArmMode.SERVO):
166
+ self._set_last_error("set_position: needs set_mode(0) or set_mode(1)")
167
+ return R1State.MODE_MISMATCH
168
+ if self.control_loop_active:
169
+ self._set_last_error("set_position: the control loop is running (a previous set_position has not "
170
+ "finished, or the user called start_control_loop)")
171
+ return R1State.NOT_READY
172
+ rad = self._is_radian if is_radian is None else bool(is_radian)
173
+ for v in (x, y, z, roll, pitch, yaw):
174
+ if v is not None and (isinstance(v, bool) or not isinstance(v, (int, float))):
175
+ return R1State.PARAM_ERROR
176
+ v_lin = DEFAULT_LIN_SPEED_MM_S if speed is None else speed
177
+ if isinstance(v_lin, bool) or not isinstance(v_lin, (int, float)) or not (0 < v_lin <= MAX_LIN_SPEED_MM_S):
178
+ return R1State.OUT_OF_RANGE
179
+ v_lin = float(v_lin) / 1000.0
180
+ if rot_speed is None:
181
+ v_rot = math.radians(DEFAULT_ROT_SPEED_DEG_S)
182
+ else:
183
+ if isinstance(rot_speed, bool) or not isinstance(rot_speed, (int, float)) or rot_speed <= 0:
184
+ return R1State.OUT_OF_RANGE
185
+ v_rot = min(MAX_ROT_SPEED_RAD_S, to_rad(float(rot_speed), rad))
186
+
187
+ q0 = self._current_q(prefer_target=True)
188
+ if q0 is None:
189
+ self._set_last_error("set_position: no fresh joint feedback, the start pose is unknown")
190
+ return R1State.FB_LOST
191
+ T0 = chain.fk(q0, self._tcp_T)
192
+ p0, rpy0 = T_to_pose(T0)
193
+ cur = [p0[0] * 1000.0, p0[1] * 1000.0, p0[2] * 1000.0] + [from_rad(a, rad) for a in rpy0]
194
+ req = [x, y, z, roll, pitch, yaw]
195
+ if relative:
196
+ goal = [c + (0.0 if v is None else float(v)) for c, v in zip(cur, req)]
197
+ else:
198
+ goal = [c if v is None else float(v) for c, v in zip(cur, req)]
199
+ T1 = self._T_from_pose(goal, rad)
200
+
201
+ # ---- plan: time, waypoints, IK pre-check
202
+ dist = float(np.linalg.norm(T1[:3, 3] - T0[:3, 3]))
203
+ ang = rot_angle_between(T0[:3, :3], T1[:3, :3])
204
+ rate = float(self._cfg.keepalive_hz)
205
+ dt = 1.0 / rate
206
+ duration = max(dist / v_lin, ang / v_rot, dt)
207
+ n = max(1, int(math.ceil(duration * rate)))
208
+ lo, hi = self._ik_bounds()
209
+ path: List[np.ndarray] = []
210
+ q_prev = np.asarray(q0, dtype=float)
211
+ for k in range(1, n + 1):
212
+ s = min_jerk(k / n)
213
+ Tk = T0.copy()
214
+ Tk[:3, 3] = T0[:3, 3] + s * (T1[:3, 3] - T0[:3, 3])
215
+ Tk[:3, :3] = rot_interp(T0[:3, :3], T1[:3, :3], s)
216
+ res = solver.solve(Tk, q_prev, self._tcp_T, lo, hi)
217
+ if not res.success:
218
+ self._set_last_error(
219
+ f"set_position: IK failed at waypoint {k}/{n}, residual {res.pos_err * 1000:.1f} mm / "
220
+ f"{math.degrees(res.rot_err):.1f} deg (joints at a limit: {[i + 1 for i in res.at_limit]})")
221
+ return R1State.IK_NO_SOLUTION
222
+ jump = float(np.max(np.abs(res.q - q_prev)))
223
+ if jump > MAX_STEP_RAD:
224
+ self._set_last_error(f"set_position: rejected, joints jump {math.degrees(jump):.0f} deg at waypoint "
225
+ f"{k}/{n} (IK branch flip)")
226
+ return R1State.IK_NO_SOLUTION
227
+ path.append(res.q)
228
+ q_prev = res.q
229
+ # per-step joint speed cap for position mode (Damiao v_des): a little above what the path needs
230
+ vlim = 0.1
231
+ for a, b in zip([np.asarray(q0)] + path[:-1], path):
232
+ vlim = max(vlim, float(np.max(np.abs(b - a))) / dt * 1.5)
233
+ vlim = min(vlim, 10.0)
234
+
235
+ # ---- stream on the control-loop thread
236
+ self._cart_done.clear()
237
+ self._cart_code = R1State.NORMAL
238
+ state = {"i": 0}
239
+
240
+ def _step(arm, _dt):
241
+ code = arm._ready_code()
242
+ if code != R1State.NORMAL:
243
+ arm._cart_code = code
244
+ arm._cart_done.set()
245
+ arm.stop_control_loop()
246
+ return
247
+ i = state["i"]
248
+ if i >= len(path):
249
+ arm._cart_done.set()
250
+ arm.stop_control_loop()
251
+ return
252
+ q = path[i]
253
+ state["i"] = i + 1
254
+ with arm._lock:
255
+ for j in range(NJ):
256
+ arm._joints[j].target = float(q[j])
257
+ if arm._mode == ArmMode.POSITION:
258
+ arm._joints[j].vlim = vlim
259
+ arm._targets_valid = True
260
+ if arm._mode == ArmMode.POSITION:
261
+ arm._send_hold_once()
262
+
263
+ code = self.start_control_loop(_step, rate)
264
+ if code != R1State.NORMAL:
265
+ self._cart_done.set()
266
+ return code
267
+ if not wait:
268
+ return R1State.NORMAL
269
+ tmo = (duration + 2.0) if timeout is None else float(timeout)
270
+ deadline = time.monotonic() + tmo
271
+ if not self._cart_done.wait(max(0.0, deadline - time.monotonic())):
272
+ return R1State.WAIT_FINISH_TIMEOUT
273
+ if self._cart_code != R1State.NORMAL:
274
+ return self._cart_code
275
+ return self._wait_settle(path[-1], deadline)
276
+
277
+ def stop_position(self) -> int:
278
+ """Abort an in-flight set_position: the arm holds the last streamed waypoint."""
279
+ self._kin()
280
+ if not self._cart_done.is_set():
281
+ self._cart_code = R1State.NOT_READY
282
+ self._cart_done.set()
283
+ self.stop_control_loop()
284
+ return R1State.NORMAL
285
+
286
+ @property
287
+ def position_moving(self) -> bool:
288
+ """True while a set_position path is still being streamed."""
289
+ self._kin()
290
+ return not self._cart_done.is_set()
291
+
292
+ # ------------------------------------------------------------------ internal
293
+ def _pose_from_T(self, T: np.ndarray, rad: bool) -> list:
294
+ p, rpy = T_to_pose(T)
295
+ return [float(p[0]) * 1000.0, float(p[1]) * 1000.0, float(p[2]) * 1000.0] + [from_rad(a, rad) for a in rpy]
296
+
297
+ @staticmethod
298
+ def _T_from_pose(pose, rad: bool) -> np.ndarray:
299
+ xyz = [float(pose[0]) / 1000.0, float(pose[1]) / 1000.0, float(pose[2]) / 1000.0]
300
+ rpy = [to_rad(float(a), rad) for a in pose[3:6]]
301
+ return pose_to_T(xyz, rpy)
302
+
303
+ def _ik_bounds(self) -> Tuple[np.ndarray, np.ndarray]:
304
+ chain, _ = self._kin()
305
+ lo = chain.lower.copy()
306
+ hi = chain.upper.copy()
307
+ if self._limits is not None:
308
+ for j, (a, b) in enumerate(self._limits):
309
+ lo[j] = max(lo[j], a)
310
+ hi[j] = min(hi[j], b)
311
+ return lo, hi
312
+
313
+ def _current_q(self, prefer_target: bool) -> Optional[np.ndarray]:
314
+ """Joint vector to start from: the commanded targets when valid, else fresh feedback, else None."""
315
+ if not self.connected:
316
+ return None
317
+ now = time.monotonic()
318
+ with self._lock:
319
+ if prefer_target and self._targets_valid:
320
+ return np.array([j.target for j in self._joints], dtype=float)
321
+ if all(j.fresh(now, 1.0) for j in self._joints):
322
+ return np.array([j.pos for j in self._joints], dtype=float)
323
+ return None
324
+
325
+ def _wait_settle(self, q_end: np.ndarray, deadline: float) -> int:
326
+ stale = self._cfg.feedback_stale_s
327
+ while time.monotonic() < deadline:
328
+ code = self._ready_code()
329
+ if code != R1State.NORMAL:
330
+ return code
331
+ now = time.monotonic()
332
+ with self._lock:
333
+ done = all(
334
+ self._joints[j].fresh(now, stale)
335
+ and abs(float(q_end[j]) - self._joints[j].pos) <= REACH_TOL_RAD
336
+ and abs(self._joints[j].vel) <= REACH_VEL_RAD
337
+ for j in range(NJ)
338
+ )
339
+ if done:
340
+ return R1State.NORMAL
341
+ time.sleep(0.02)
342
+ return R1State.WAIT_FINISH_TIMEOUT
@@ -0,0 +1,105 @@
1
+ """Reader thread for the bottom board's USART2 log port (115200).
2
+
3
+ The log port is the user's own USB-serial adapter, not the board's USB CDC. The main path for board state is the
4
+ local reports on the USB-CAN bridge (transport.REP_LOCAL_*); this port is optional: when connected you also get the
5
+ "arm: ..." text log, and ``$S/$E`` lines are parsed into the same BoardStatus/BoardEvent and fed into Base.
6
+ """
7
+
8
+ from __future__ import annotations
9
+
10
+ import threading
11
+ import time
12
+ from typing import Callable, List, Optional
13
+
14
+ import serial
15
+
16
+ from .protocol import BoardEvent, BoardStatus, LogLine, parse_log_line
17
+
18
+ LineCallback = Callable[[object], None] # BoardStatus | BoardEvent | LogLine
19
+
20
+
21
+ class LogPort:
22
+ def __init__(self, port: str, baud: int = 115200) -> None:
23
+ self.port = port
24
+ self.baud = baud
25
+ self._ser: Optional[serial.Serial] = None
26
+ self._thread: Optional[threading.Thread] = None
27
+ self._stop = threading.Event()
28
+ self._callbacks: List[LineCallback] = []
29
+ self.lines = 0
30
+ self.bad_lines = 0
31
+ self.last_status: Optional[BoardStatus] = None
32
+ self.last_event: Optional[BoardEvent] = None
33
+
34
+ @property
35
+ def is_open(self) -> bool:
36
+ return self._ser is not None and self._ser.is_open
37
+
38
+ def open(self) -> None:
39
+ if self.is_open:
40
+ return
41
+ self._ser = serial.Serial(self.port, self.baud, timeout=0.05)
42
+ self._ser.reset_input_buffer()
43
+ self._stop.clear()
44
+ self._thread = threading.Thread(target=self._loop, name="r1-logport", daemon=True)
45
+ self._thread.start()
46
+
47
+ def close(self) -> None:
48
+ self._stop.set()
49
+ t = self._thread
50
+ if t is not None and t.is_alive() and t is not threading.current_thread():
51
+ t.join(1.0)
52
+ self._thread = None
53
+ ser = self._ser
54
+ self._ser = None
55
+ if ser is not None:
56
+ try:
57
+ ser.close()
58
+ except Exception:
59
+ pass
60
+
61
+ def add_callback(self, cb: LineCallback) -> None:
62
+ self._callbacks.append(cb)
63
+
64
+ def remove_callback(self, cb: LineCallback) -> None:
65
+ if cb in self._callbacks:
66
+ self._callbacks.remove(cb)
67
+
68
+ def _loop(self) -> None:
69
+ buf = bytearray()
70
+ while not self._stop.is_set():
71
+ ser = self._ser
72
+ if ser is None:
73
+ break
74
+ try:
75
+ chunk = ser.read(ser.in_waiting or 1)
76
+ except Exception:
77
+ break
78
+ if not chunk:
79
+ continue
80
+ buf += chunk
81
+ now = time.monotonic()
82
+ while True:
83
+ k = buf.find(b"\n")
84
+ if k < 0:
85
+ break
86
+ raw = bytes(buf[:k])
87
+ del buf[:k + 1]
88
+ line = raw.decode("utf-8", errors="replace").rstrip("\r")
89
+ self.lines += 1
90
+ parsed = parse_log_line(line, now)
91
+ if parsed is None:
92
+ if line.startswith("$"):
93
+ self.bad_lines += 1
94
+ continue
95
+ if isinstance(parsed, BoardStatus):
96
+ self.last_status = parsed
97
+ elif isinstance(parsed, BoardEvent):
98
+ self.last_event = parsed
99
+ for cb in list(self._callbacks):
100
+ try:
101
+ cb(parsed)
102
+ except Exception:
103
+ pass
104
+ if len(buf) > 8192:
105
+ del buf[:-1024]