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