a2a-robots-pollen 0.1.1__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,28 @@
|
|
|
1
|
+
"""Pollen Robotics Reachy 2 backend for a2a-robots."""
|
|
2
|
+
|
|
3
|
+
__version__ = "0.1.1"
|
|
4
|
+
|
|
5
|
+
from a2a_robots_pollen.backend import PollenBackend, Reachy2Backend, connect
|
|
6
|
+
from a2a_robots_pollen.fake import FakeReachySDK
|
|
7
|
+
from a2a_robots_pollen.planner import (
|
|
8
|
+
ARM_JOINTS,
|
|
9
|
+
Reachy2Planner,
|
|
10
|
+
hand_joints,
|
|
11
|
+
matrix_to_pose,
|
|
12
|
+
pose_error,
|
|
13
|
+
pose_to_matrix,
|
|
14
|
+
)
|
|
15
|
+
|
|
16
|
+
__all__ = [
|
|
17
|
+
"ARM_JOINTS",
|
|
18
|
+
"FakeReachySDK",
|
|
19
|
+
"PollenBackend",
|
|
20
|
+
"Reachy2Backend",
|
|
21
|
+
"Reachy2Planner",
|
|
22
|
+
"__version__",
|
|
23
|
+
"connect",
|
|
24
|
+
"hand_joints",
|
|
25
|
+
"matrix_to_pose",
|
|
26
|
+
"pose_error",
|
|
27
|
+
"pose_to_matrix",
|
|
28
|
+
]
|
|
@@ -0,0 +1,422 @@
|
|
|
1
|
+
"""Pollen Robotics Reachy 2 backend for a2a-robots."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
import math
|
|
6
|
+
import threading
|
|
7
|
+
import time
|
|
8
|
+
from collections.abc import Callable, Iterable, Sequence
|
|
9
|
+
from threading import Event
|
|
10
|
+
from typing import Any
|
|
11
|
+
|
|
12
|
+
import numpy as np
|
|
13
|
+
|
|
14
|
+
from a2a_robots import FailureCode, HandState, MotionOutcome, Pose, RobotBackend
|
|
15
|
+
from a2a_robots.planning import PlanError, Planner
|
|
16
|
+
from a2a_robots_pollen.planner import (
|
|
17
|
+
Reachy2Planner,
|
|
18
|
+
hand_joints,
|
|
19
|
+
matrix_to_pose,
|
|
20
|
+
pose_error,
|
|
21
|
+
)
|
|
22
|
+
|
|
23
|
+
HAND_ATTRS: dict[str, str] = {"left": "l_arm", "right": "r_arm"}
|
|
24
|
+
|
|
25
|
+
|
|
26
|
+
def _clamp_opening(pct: float) -> float:
|
|
27
|
+
return min(100.0, max(0.0, float(pct)))
|
|
28
|
+
|
|
29
|
+
|
|
30
|
+
_REACHY_PORT_KWARGS = frozenset({"sdk_port", "audio_port", "video_port"})
|
|
31
|
+
|
|
32
|
+
|
|
33
|
+
def connect(host: str = "localhost", **kwargs: Any) -> Any:
|
|
34
|
+
"""Connect to a live or simulated Reachy 2 gRPC server.
|
|
35
|
+
|
|
36
|
+
Extra kwargs such as ``sdk_port``, ``audio_port``, and ``video_port`` are
|
|
37
|
+
forwarded to ``ReachySDK.__init__``. In ``reachy2-sdk`` 1.0.15,
|
|
38
|
+
``ReachySDK.__new__`` only accepts ``host`` and ``fake_only``, so those
|
|
39
|
+
port kwargs are applied via an explicit ``__new__``/``__init__`` path.
|
|
40
|
+
"""
|
|
41
|
+
try:
|
|
42
|
+
from reachy2_sdk import ReachySDK
|
|
43
|
+
except ModuleNotFoundError as exc:
|
|
44
|
+
raise ModuleNotFoundError(
|
|
45
|
+
"reachy2-sdk is required to connect to a Reachy 2 robot; "
|
|
46
|
+
"install it with `pip install reachy2-sdk`"
|
|
47
|
+
) from exc
|
|
48
|
+
|
|
49
|
+
if _REACHY_PORT_KWARGS.intersection(kwargs):
|
|
50
|
+
# In reachy2-sdk 1.0.15, __init__ accepts sdk_port/audio_port/video_port,
|
|
51
|
+
# while __new__(cls, host, fake_only=False) rejects those kwargs.
|
|
52
|
+
reachy = ReachySDK.__new__(ReachySDK, host, fake_only=bool(kwargs.get("fake_only", False)))
|
|
53
|
+
ReachySDK.__init__(reachy, host=host, **kwargs)
|
|
54
|
+
else:
|
|
55
|
+
reachy = ReachySDK(host=host, **kwargs)
|
|
56
|
+
if hasattr(reachy, "is_connected") and not reachy.is_connected():
|
|
57
|
+
raise ConnectionError(f"failed to connect to Reachy 2 gRPC server at {host!r}")
|
|
58
|
+
return reachy
|
|
59
|
+
|
|
60
|
+
|
|
61
|
+
class Reachy2Backend(RobotBackend):
|
|
62
|
+
"""Drives Pollen Robotics Reachy 2 arms and grippers through ``reachy2-sdk``."""
|
|
63
|
+
|
|
64
|
+
def __init__(
|
|
65
|
+
self,
|
|
66
|
+
reachy: Any,
|
|
67
|
+
*,
|
|
68
|
+
hands: Iterable[str] = ("left", "right"),
|
|
69
|
+
frame: str = "world",
|
|
70
|
+
realtime: bool = True,
|
|
71
|
+
control_dt: float = 0.02,
|
|
72
|
+
settle_time_s: float = 1.0,
|
|
73
|
+
position_tolerance_m: float = 0.005,
|
|
74
|
+
orientation_tolerance_rad: float = 0.05,
|
|
75
|
+
max_speed_rad_s: float = 0.25,
|
|
76
|
+
max_joint_delta_rad: float = 1.0,
|
|
77
|
+
closed_opening_pct: float = 2.0,
|
|
78
|
+
open_opening_pct: float = 95.0,
|
|
79
|
+
gripper_step_pct: float = 10.0,
|
|
80
|
+
min_gripper_steps: int = 5,
|
|
81
|
+
contact_stall_s: float = 0.1,
|
|
82
|
+
grasp_settle_s: float = 0.2,
|
|
83
|
+
startup_grace_s: float = 0.2,
|
|
84
|
+
grasp_opening_tol_pct: float = 5.0,
|
|
85
|
+
turn_on: bool = True,
|
|
86
|
+
planner: Planner | None = None,
|
|
87
|
+
) -> None:
|
|
88
|
+
"""``contact_stall_s`` is how long the gripper must stop closing short of its
|
|
89
|
+
target before ``close`` counts it as touching something; shorter stalls (a
|
|
90
|
+
late state update, brief friction) are ignored. ``grasp_settle_s`` is how
|
|
91
|
+
long it must stay stalled to count as a grasp. ``startup_grace_s`` ignores
|
|
92
|
+
stalls at the start of ``close`` while the opening is still at its start
|
|
93
|
+
value (mid-range start plus brief latency). After the grace, or once the
|
|
94
|
+
opening has moved, stalls count — including a re-close that is already on
|
|
95
|
+
an object. A stall that passes the first threshold but then closes past
|
|
96
|
+
the object before the second is ``slipped``. The gripper only reports its
|
|
97
|
+
opening, so a lag lasting between the two durations that then catches up
|
|
98
|
+
in empty air also reads as ``slipped``. ``grasp_opening_tol_pct`` is how
|
|
99
|
+
far the measured opening may drift from the opening recorded at grasp
|
|
100
|
+
settle before ``held`` clears and ``hold`` stops re-commanding a squeeze."""
|
|
101
|
+
if hasattr(reachy, "is_connected") and not reachy.is_connected():
|
|
102
|
+
raise ConnectionError("Reachy SDK instance is not connected")
|
|
103
|
+
if control_dt <= 0.0:
|
|
104
|
+
raise ValueError("control_dt must be positive")
|
|
105
|
+
if settle_time_s < 0.0:
|
|
106
|
+
raise ValueError("settle_time_s must be non-negative")
|
|
107
|
+
if position_tolerance_m <= 0.0:
|
|
108
|
+
raise ValueError("position_tolerance_m must be positive")
|
|
109
|
+
if orientation_tolerance_rad <= 0.0:
|
|
110
|
+
raise ValueError("orientation_tolerance_rad must be positive")
|
|
111
|
+
if gripper_step_pct <= 0.0:
|
|
112
|
+
raise ValueError("gripper_step_pct must be positive")
|
|
113
|
+
if not (0.0 <= closed_opening_pct < open_opening_pct <= 100.0):
|
|
114
|
+
raise ValueError("expected 0.0 <= closed_opening_pct < open_opening_pct <= 100.0")
|
|
115
|
+
if contact_stall_s <= 0.0:
|
|
116
|
+
raise ValueError("contact_stall_s must be positive")
|
|
117
|
+
if grasp_settle_s <= contact_stall_s:
|
|
118
|
+
raise ValueError("grasp_settle_s must be greater than contact_stall_s")
|
|
119
|
+
if startup_grace_s < 0.0:
|
|
120
|
+
raise ValueError("startup_grace_s must be non-negative")
|
|
121
|
+
if grasp_opening_tol_pct < 0.0:
|
|
122
|
+
raise ValueError("grasp_opening_tol_pct must be non-negative")
|
|
123
|
+
|
|
124
|
+
configured_hands = tuple(hands)
|
|
125
|
+
if not configured_hands:
|
|
126
|
+
raise ValueError("at least one hand must be configured")
|
|
127
|
+
|
|
128
|
+
arms: dict[str, Any] = {}
|
|
129
|
+
for hand in configured_hands:
|
|
130
|
+
attr = HAND_ATTRS.get(hand)
|
|
131
|
+
if attr is None:
|
|
132
|
+
raise ValueError(f"unsupported hand {hand!r}; expected 'left' or 'right'")
|
|
133
|
+
arm = getattr(reachy, attr, None)
|
|
134
|
+
if arm is None:
|
|
135
|
+
raise ValueError(f"Reachy instance has no active arm for hand {hand!r}")
|
|
136
|
+
if getattr(arm, "gripper", None) is None:
|
|
137
|
+
raise ValueError(f"Reachy arm {hand!r} has no gripper")
|
|
138
|
+
if turn_on and hasattr(arm, "is_on") and not arm.is_on():
|
|
139
|
+
if not arm.turn_on():
|
|
140
|
+
raise RuntimeError(f"failed to turn on Reachy arm {hand!r}")
|
|
141
|
+
arms[hand] = arm
|
|
142
|
+
|
|
143
|
+
self._reachy = reachy
|
|
144
|
+
self._hands = frozenset(configured_hands)
|
|
145
|
+
self._arms = arms
|
|
146
|
+
self._frame = frame
|
|
147
|
+
self._realtime = realtime
|
|
148
|
+
self._control_dt = control_dt
|
|
149
|
+
self._settle_steps = max(1, round(settle_time_s / control_dt))
|
|
150
|
+
self._position_tolerance_m = position_tolerance_m
|
|
151
|
+
self._orientation_tolerance_rad = orientation_tolerance_rad
|
|
152
|
+
self._closed_opening_pct = closed_opening_pct
|
|
153
|
+
self._open_opening_pct = open_opening_pct
|
|
154
|
+
self._gripper_step_pct = gripper_step_pct
|
|
155
|
+
self._min_gripper_steps = max(1, min_gripper_steps)
|
|
156
|
+
self._contact_stall_steps = max(2, round(contact_stall_s / control_dt))
|
|
157
|
+
self._grasp_settle_steps = max(
|
|
158
|
+
self._contact_stall_steps + 1, round(grasp_settle_s / control_dt)
|
|
159
|
+
)
|
|
160
|
+
self._startup_grace_steps = max(0, round(startup_grace_s / control_dt))
|
|
161
|
+
self._grasp_opening_tol_pct = grasp_opening_tol_pct
|
|
162
|
+
self._lock = threading.Lock()
|
|
163
|
+
self._closed_hands: set[str] = set()
|
|
164
|
+
self._grasp_opening: dict[str, float] = {}
|
|
165
|
+
self._planner = planner or Reachy2Planner(
|
|
166
|
+
arms,
|
|
167
|
+
frame=frame,
|
|
168
|
+
max_speed_rad_s=max_speed_rad_s,
|
|
169
|
+
max_joint_delta_rad=max_joint_delta_rad,
|
|
170
|
+
position_tolerance_m=position_tolerance_m,
|
|
171
|
+
orientation_tolerance_rad=orientation_tolerance_rad,
|
|
172
|
+
)
|
|
173
|
+
|
|
174
|
+
@property
|
|
175
|
+
def hands(self) -> frozenset[str]:
|
|
176
|
+
return self._hands
|
|
177
|
+
|
|
178
|
+
@staticmethod
|
|
179
|
+
def _arm_joint_objects(arm: Any) -> tuple[Any, ...]:
|
|
180
|
+
return (
|
|
181
|
+
arm.shoulder.pitch,
|
|
182
|
+
arm.shoulder.roll,
|
|
183
|
+
arm.elbow.yaw,
|
|
184
|
+
arm.elbow.pitch,
|
|
185
|
+
arm.wrist.roll,
|
|
186
|
+
arm.wrist.pitch,
|
|
187
|
+
arm.wrist.yaw,
|
|
188
|
+
)
|
|
189
|
+
|
|
190
|
+
def _read_arm_radians(self, hand: str) -> list[float]:
|
|
191
|
+
return [float(v) for v in self._arms[hand].get_current_positions(degrees=False)]
|
|
192
|
+
|
|
193
|
+
def _write_arm_radians(self, hand: str, q_rad: Sequence[float]) -> None:
|
|
194
|
+
arm = self._arms[hand]
|
|
195
|
+
joints = self._arm_joint_objects(arm)
|
|
196
|
+
for joint, angle in zip(joints, q_rad, strict=True):
|
|
197
|
+
joint.goal_position = math.degrees(float(angle))
|
|
198
|
+
arm.send_goal_positions(check_positions=False)
|
|
199
|
+
|
|
200
|
+
def _hand_pose(self, hand: str) -> Pose:
|
|
201
|
+
arm = self._arms[hand]
|
|
202
|
+
# Pass explicit 7-element joint angles in radians: in reachy2-sdk 1.0.15,
|
|
203
|
+
# forward_kinematics(None, degrees=False) reads cached degree values
|
|
204
|
+
# (including the gripper in arm._actuators) without converting to radians.
|
|
205
|
+
q_rad = self._read_arm_radians(hand)
|
|
206
|
+
matrix = np.asarray(arm.forward_kinematics(q_rad, degrees=False), dtype=np.float64)
|
|
207
|
+
return matrix_to_pose(matrix, self._frame)
|
|
208
|
+
|
|
209
|
+
def _gripper_opening(self, hand: str) -> float:
|
|
210
|
+
gripper = self._arms[hand].gripper
|
|
211
|
+
opening = gripper.opening
|
|
212
|
+
if opening is None and hasattr(gripper, "get_current_opening"):
|
|
213
|
+
opening = gripper.get_current_opening()
|
|
214
|
+
return float(opening)
|
|
215
|
+
|
|
216
|
+
def _still_grasping(self, hand: str, opening: float) -> bool:
|
|
217
|
+
"""True while a settled close still has the fingers near the grasp opening."""
|
|
218
|
+
grasp_at = self._grasp_opening.get(hand)
|
|
219
|
+
return (
|
|
220
|
+
hand in self._closed_hands
|
|
221
|
+
and grasp_at is not None
|
|
222
|
+
and abs(opening - grasp_at) <= self._grasp_opening_tol_pct
|
|
223
|
+
and self._closed_opening_pct < opening < self._open_opening_pct
|
|
224
|
+
)
|
|
225
|
+
|
|
226
|
+
def _grasp_state(self, hand: str) -> tuple[bool, dict[str, float]]:
|
|
227
|
+
opening = self._gripper_opening(hand)
|
|
228
|
+
if not self._still_grasping(hand, opening):
|
|
229
|
+
return False, {}
|
|
230
|
+
# ParallelGripper does not report measured contact force, so expose an
|
|
231
|
+
# opening-derived proxy estimate rather than a physical sensor reading.
|
|
232
|
+
estimated_force = round((100.0 - opening) / 10.0, 4)
|
|
233
|
+
return True, {"finger": estimated_force}
|
|
234
|
+
|
|
235
|
+
def observe(self) -> dict[str, HandState]:
|
|
236
|
+
with self._lock:
|
|
237
|
+
states: dict[str, HandState] = {}
|
|
238
|
+
for hand in self._hands:
|
|
239
|
+
pose = self._hand_pose(hand)
|
|
240
|
+
held, contact = self._grasp_state(hand)
|
|
241
|
+
states[hand] = HandState(pose=pose, contact=contact, held=held)
|
|
242
|
+
return states
|
|
243
|
+
|
|
244
|
+
def move_to(self, hand: str, target: Pose, stop: Event) -> MotionOutcome:
|
|
245
|
+
expected_joints = hand_joints(hand)
|
|
246
|
+
with self._lock:
|
|
247
|
+
q0 = self._read_arm_radians(hand)
|
|
248
|
+
start = dict(zip(expected_joints, q0, strict=True))
|
|
249
|
+
try:
|
|
250
|
+
trajectory = self._planner.plan(hand, start, target, self._control_dt)
|
|
251
|
+
except PlanError as error:
|
|
252
|
+
return MotionOutcome(error.failure_code, error.detail)
|
|
253
|
+
|
|
254
|
+
if tuple(trajectory.joints) != expected_joints:
|
|
255
|
+
raise ValueError(
|
|
256
|
+
f"planner returned joints {trajectory.joints!r}, expected {expected_joints!r}"
|
|
257
|
+
)
|
|
258
|
+
|
|
259
|
+
points = trajectory.points
|
|
260
|
+
|
|
261
|
+
def track(step: int) -> bool:
|
|
262
|
+
point = points[min(step, len(points) - 1)]
|
|
263
|
+
self._write_arm_radians(hand, point)
|
|
264
|
+
if step < len(points) - 1:
|
|
265
|
+
return False
|
|
266
|
+
pos_err, ori_err = pose_error(self._hand_pose(hand), target)
|
|
267
|
+
return (
|
|
268
|
+
pos_err <= self._position_tolerance_m and ori_err <= self._orientation_tolerance_rad
|
|
269
|
+
)
|
|
270
|
+
|
|
271
|
+
settled = self._run(stop, track, extra_steps=len(points))
|
|
272
|
+
if stop.is_set():
|
|
273
|
+
return MotionOutcome(FailureCode.STOPPED, "motion stopped before completion")
|
|
274
|
+
if not settled:
|
|
275
|
+
return MotionOutcome(FailureCode.UNREACHABLE, "the hand did not settle at the target")
|
|
276
|
+
return MotionOutcome()
|
|
277
|
+
|
|
278
|
+
def open(self, hand: str, stop: Event) -> MotionOutcome:
|
|
279
|
+
with self._lock:
|
|
280
|
+
start_opening = _clamp_opening(self._gripper_opening(hand))
|
|
281
|
+
|
|
282
|
+
steps = max(
|
|
283
|
+
self._min_gripper_steps,
|
|
284
|
+
math.ceil(abs(100.0 - start_opening) / self._gripper_step_pct),
|
|
285
|
+
)
|
|
286
|
+
gripper = self._arms[hand].gripper
|
|
287
|
+
|
|
288
|
+
def track(step: int) -> bool:
|
|
289
|
+
fraction = min(1.0, (step + 1) / steps)
|
|
290
|
+
target_pct = _clamp_opening(start_opening + (100.0 - start_opening) * fraction)
|
|
291
|
+
gripper.set_opening(target_pct)
|
|
292
|
+
return step >= steps - 1 and self._gripper_opening(hand) >= self._open_opening_pct
|
|
293
|
+
|
|
294
|
+
settled = self._run(stop, track, extra_steps=steps)
|
|
295
|
+
if stop.is_set():
|
|
296
|
+
return MotionOutcome(FailureCode.STOPPED, "motion stopped before completion")
|
|
297
|
+
if not settled:
|
|
298
|
+
return MotionOutcome(FailureCode.TIMEOUT, "the gripper did not settle")
|
|
299
|
+
with self._lock:
|
|
300
|
+
# Clear grasp state only after a successful open, so a stopped or
|
|
301
|
+
# timed-out open does not claim the hand released an object.
|
|
302
|
+
self._closed_hands.discard(hand)
|
|
303
|
+
self._grasp_opening.pop(hand, None)
|
|
304
|
+
return MotionOutcome()
|
|
305
|
+
|
|
306
|
+
def close(self, hand: str, stop: Event) -> MotionOutcome:
|
|
307
|
+
with self._lock:
|
|
308
|
+
start_opening = _clamp_opening(self._gripper_opening(hand))
|
|
309
|
+
|
|
310
|
+
steps = max(
|
|
311
|
+
self._min_gripper_steps,
|
|
312
|
+
math.ceil(abs(start_opening) / self._gripper_step_pct),
|
|
313
|
+
)
|
|
314
|
+
gripper = self._arms[hand].gripper
|
|
315
|
+
touched = False
|
|
316
|
+
moved_from_start = False
|
|
317
|
+
stalled_steps = 0
|
|
318
|
+
prev_actual_pct: float | None = None
|
|
319
|
+
|
|
320
|
+
def track(step: int) -> bool:
|
|
321
|
+
nonlocal touched, moved_from_start, stalled_steps, prev_actual_pct
|
|
322
|
+
fraction = min(1.0, (step + 1) / steps)
|
|
323
|
+
target_pct = _clamp_opening(start_opening * (1.0 - fraction))
|
|
324
|
+
gripper.set_opening(target_pct)
|
|
325
|
+
actual_pct = self._gripper_opening(hand)
|
|
326
|
+
if abs(actual_pct - start_opening) >= 0.5:
|
|
327
|
+
moved_from_start = True
|
|
328
|
+
# During start-up grace, ignore stalls only while still at
|
|
329
|
+
# start_opening (mid-range start + brief latency). Once the opening
|
|
330
|
+
# has moved, or the grace has elapsed, stalls count — including a
|
|
331
|
+
# re-close that is already on an object and never leaves start.
|
|
332
|
+
if (
|
|
333
|
+
(step >= self._startup_grace_steps or moved_from_start)
|
|
334
|
+
and prev_actual_pct is not None
|
|
335
|
+
and abs(actual_pct - prev_actual_pct) < 0.5
|
|
336
|
+
and actual_pct - target_pct > 1.0
|
|
337
|
+
and self._closed_opening_pct < actual_pct < self._open_opening_pct
|
|
338
|
+
):
|
|
339
|
+
stalled_steps += 1
|
|
340
|
+
else:
|
|
341
|
+
stalled_steps = 0
|
|
342
|
+
prev_actual_pct = actual_pct
|
|
343
|
+
# Only a sustained stall counts as contact, so a single stale reading
|
|
344
|
+
# or a brief lag that then catches up is not reported as a slip.
|
|
345
|
+
if stalled_steps >= self._contact_stall_steps:
|
|
346
|
+
touched = True
|
|
347
|
+
return step >= steps - 1 and (
|
|
348
|
+
actual_pct <= self._closed_opening_pct or stalled_steps >= self._grasp_settle_steps
|
|
349
|
+
)
|
|
350
|
+
|
|
351
|
+
settled = self._run(stop, track, extra_steps=steps)
|
|
352
|
+
if stop.is_set():
|
|
353
|
+
return MotionOutcome(FailureCode.STOPPED, "motion stopped before completion")
|
|
354
|
+
if not settled:
|
|
355
|
+
return MotionOutcome(FailureCode.TIMEOUT, "the gripper did not settle")
|
|
356
|
+
|
|
357
|
+
with self._lock:
|
|
358
|
+
# Mark the hand closed only after settle, so a stopped mid-close
|
|
359
|
+
# never reports held=True from a partial opening.
|
|
360
|
+
opening = self._gripper_opening(hand)
|
|
361
|
+
self._closed_hands.add(hand)
|
|
362
|
+
self._grasp_opening[hand] = opening
|
|
363
|
+
held, _ = self._grasp_state(hand)
|
|
364
|
+
if touched and not held:
|
|
365
|
+
return MotionOutcome(FailureCode.SLIPPED, "object slipped during the grasp")
|
|
366
|
+
return MotionOutcome()
|
|
367
|
+
|
|
368
|
+
def hold(self) -> None:
|
|
369
|
+
with self._lock:
|
|
370
|
+
# Attempt every hand even if one raises: stop/cancel must freeze both
|
|
371
|
+
# arms. Re-raise the first error after the loop.
|
|
372
|
+
errors: list[Exception] = []
|
|
373
|
+
for hand in self._hands:
|
|
374
|
+
try:
|
|
375
|
+
arm = self._arms[hand]
|
|
376
|
+
if hasattr(arm, "cancel_all_goto"):
|
|
377
|
+
arm.cancel_all_goto()
|
|
378
|
+
if hasattr(arm.gripper, "cancel_all_goto"):
|
|
379
|
+
arm.gripper.cancel_all_goto()
|
|
380
|
+
self._write_arm_radians(hand, self._read_arm_radians(hand))
|
|
381
|
+
measured = _clamp_opening(self._gripper_opening(hand))
|
|
382
|
+
# Re-command a closed squeeze only while still near the grasp
|
|
383
|
+
# opening. After a stopped/timed-out open the fingers may be
|
|
384
|
+
# far from that width; holding the measured opening avoids
|
|
385
|
+
# slamming the gripper shut.
|
|
386
|
+
if self._still_grasping(hand, measured):
|
|
387
|
+
arm.gripper.set_opening(0.0)
|
|
388
|
+
else:
|
|
389
|
+
arm.gripper.set_opening(measured)
|
|
390
|
+
except Exception as error:
|
|
391
|
+
errors.append(error)
|
|
392
|
+
if errors:
|
|
393
|
+
raise errors[0]
|
|
394
|
+
|
|
395
|
+
def shutdown(self) -> None:
|
|
396
|
+
with self._lock:
|
|
397
|
+
if (
|
|
398
|
+
hasattr(self._reachy, "is_connected")
|
|
399
|
+
and self._reachy.is_connected()
|
|
400
|
+
and hasattr(self._reachy, "disconnect")
|
|
401
|
+
):
|
|
402
|
+
self._reachy.disconnect()
|
|
403
|
+
|
|
404
|
+
def _run(self, stop: Event, step_fn: Callable[[int], bool], extra_steps: int = 0) -> bool:
|
|
405
|
+
start = time.monotonic()
|
|
406
|
+
budget = self._settle_steps + extra_steps
|
|
407
|
+
for step in range(budget):
|
|
408
|
+
# Poll stop outside self._lock so concurrent observe() never deadlocks.
|
|
409
|
+
if stop.is_set():
|
|
410
|
+
return False
|
|
411
|
+
with self._lock:
|
|
412
|
+
done = step_fn(step)
|
|
413
|
+
if done:
|
|
414
|
+
return True
|
|
415
|
+
if self._realtime:
|
|
416
|
+
remaining = start + (step + 1) * self._control_dt - time.monotonic()
|
|
417
|
+
if remaining > 0 and stop.wait(remaining):
|
|
418
|
+
return False
|
|
419
|
+
return False
|
|
420
|
+
|
|
421
|
+
|
|
422
|
+
PollenBackend = Reachy2Backend
|
|
@@ -0,0 +1,406 @@
|
|
|
1
|
+
"""Offline fake Reachy 2 SDK double for tests and local examples."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
import math
|
|
6
|
+
from collections.abc import Sequence
|
|
7
|
+
from dataclasses import dataclass
|
|
8
|
+
|
|
9
|
+
import numpy as np
|
|
10
|
+
|
|
11
|
+
from a2a_robots_pollen.planner import matrix_to_pose, pose_error
|
|
12
|
+
|
|
13
|
+
_UPPER_ARM_M = 0.28
|
|
14
|
+
_FOREARM_M = 0.28
|
|
15
|
+
_TOOL_M = 0.08
|
|
16
|
+
_DEFAULT_Q_RAD: tuple[float, ...] = (-0.4, 0.0, 0.0, -1.3, 0.0, 0.1, 0.0)
|
|
17
|
+
|
|
18
|
+
|
|
19
|
+
def _rot_x(theta: float) -> np.ndarray:
|
|
20
|
+
c, s = math.cos(theta), math.sin(theta)
|
|
21
|
+
return np.array(
|
|
22
|
+
[
|
|
23
|
+
[1.0, 0.0, 0.0, 0.0],
|
|
24
|
+
[0.0, c, -s, 0.0],
|
|
25
|
+
[0.0, s, c, 0.0],
|
|
26
|
+
[0.0, 0.0, 0.0, 1.0],
|
|
27
|
+
],
|
|
28
|
+
dtype=np.float64,
|
|
29
|
+
)
|
|
30
|
+
|
|
31
|
+
|
|
32
|
+
def _rot_y(theta: float) -> np.ndarray:
|
|
33
|
+
c, s = math.cos(theta), math.sin(theta)
|
|
34
|
+
return np.array(
|
|
35
|
+
[
|
|
36
|
+
[c, 0.0, s, 0.0],
|
|
37
|
+
[0.0, 1.0, 0.0, 0.0],
|
|
38
|
+
[-s, 0.0, c, 0.0],
|
|
39
|
+
[0.0, 0.0, 0.0, 1.0],
|
|
40
|
+
],
|
|
41
|
+
dtype=np.float64,
|
|
42
|
+
)
|
|
43
|
+
|
|
44
|
+
|
|
45
|
+
def _rot_z(theta: float) -> np.ndarray:
|
|
46
|
+
c, s = math.cos(theta), math.sin(theta)
|
|
47
|
+
return np.array(
|
|
48
|
+
[
|
|
49
|
+
[c, -s, 0.0, 0.0],
|
|
50
|
+
[s, c, 0.0, 0.0],
|
|
51
|
+
[0.0, 0.0, 1.0, 0.0],
|
|
52
|
+
[0.0, 0.0, 0.0, 1.0],
|
|
53
|
+
],
|
|
54
|
+
dtype=np.float64,
|
|
55
|
+
)
|
|
56
|
+
|
|
57
|
+
|
|
58
|
+
def _trans(x: float, y: float, z: float) -> np.ndarray:
|
|
59
|
+
mat = np.eye(4, dtype=np.float64)
|
|
60
|
+
mat[:3, 3] = [x, y, z]
|
|
61
|
+
return mat
|
|
62
|
+
|
|
63
|
+
|
|
64
|
+
def _orientation_error_vector(current: np.ndarray, target: np.ndarray) -> np.ndarray:
|
|
65
|
+
"""Return a 3D axis-angle vector rotating ``current`` to ``target``."""
|
|
66
|
+
r_err = target @ current.T
|
|
67
|
+
skew = 0.5 * np.array(
|
|
68
|
+
[
|
|
69
|
+
r_err[2, 1] - r_err[1, 2],
|
|
70
|
+
r_err[0, 2] - r_err[2, 0],
|
|
71
|
+
r_err[1, 0] - r_err[0, 1],
|
|
72
|
+
],
|
|
73
|
+
dtype=np.float64,
|
|
74
|
+
)
|
|
75
|
+
sin_norm = float(np.linalg.norm(skew))
|
|
76
|
+
cos_val = 0.5 * (float(np.trace(r_err)) - 1.0)
|
|
77
|
+
cos_val = max(-1.0, min(1.0, cos_val))
|
|
78
|
+
if sin_norm < 1e-9:
|
|
79
|
+
if cos_val >= 0.0:
|
|
80
|
+
return np.zeros(3, dtype=np.float64)
|
|
81
|
+
# 180-degree rotation fallback along largest diagonal axis
|
|
82
|
+
diag = np.maximum(0.0, np.diag(r_err) + 1.0)
|
|
83
|
+
axis = np.sqrt(diag / 2.0)
|
|
84
|
+
return math.pi * axis
|
|
85
|
+
angle = math.atan2(sin_norm, cos_val)
|
|
86
|
+
return skew * (angle / sin_norm)
|
|
87
|
+
|
|
88
|
+
|
|
89
|
+
@dataclass
|
|
90
|
+
class FakeGraspableObject:
|
|
91
|
+
"""An object in the fake Reachy 2 scene that a gripper can stall against."""
|
|
92
|
+
|
|
93
|
+
name: str
|
|
94
|
+
position: tuple[float, float, float]
|
|
95
|
+
opening_pct: float = 35.0
|
|
96
|
+
slip: bool = False
|
|
97
|
+
capture_radius_m: float = 0.05
|
|
98
|
+
held_by: str | None = None
|
|
99
|
+
# A slipping object holds the fingers for this many control steps, long
|
|
100
|
+
# enough to count as contact but too short to count as a grasp.
|
|
101
|
+
slip_after_steps: int = 7
|
|
102
|
+
|
|
103
|
+
|
|
104
|
+
class FakeOrbitaJoint:
|
|
105
|
+
"""In-memory stand-in for ``reachy2_sdk.orbita.orbita_joint.OrbitaJoint``."""
|
|
106
|
+
|
|
107
|
+
def __init__(self, initial_deg: float = 0.0) -> None:
|
|
108
|
+
self.present_position: float = float(initial_deg)
|
|
109
|
+
self.goal_position: float = float(initial_deg)
|
|
110
|
+
|
|
111
|
+
|
|
112
|
+
class FakeShoulder:
|
|
113
|
+
def __init__(self, pitch: FakeOrbitaJoint, roll: FakeOrbitaJoint) -> None:
|
|
114
|
+
self.pitch = pitch
|
|
115
|
+
self.roll = roll
|
|
116
|
+
|
|
117
|
+
|
|
118
|
+
class FakeElbow:
|
|
119
|
+
def __init__(self, yaw: FakeOrbitaJoint, pitch: FakeOrbitaJoint) -> None:
|
|
120
|
+
self.yaw = yaw
|
|
121
|
+
self.pitch = pitch
|
|
122
|
+
|
|
123
|
+
|
|
124
|
+
class FakeWrist:
|
|
125
|
+
def __init__(self, roll: FakeOrbitaJoint, pitch: FakeOrbitaJoint, yaw: FakeOrbitaJoint) -> None:
|
|
126
|
+
self.roll = roll
|
|
127
|
+
self.pitch = pitch
|
|
128
|
+
self.yaw = yaw
|
|
129
|
+
|
|
130
|
+
|
|
131
|
+
class FakeGripper:
|
|
132
|
+
"""In-memory stand-in for ``reachy2_sdk.grippers.parallel_gripper.ParallelGripper``."""
|
|
133
|
+
|
|
134
|
+
def __init__(self, arm: FakeArm, *, opening: float = 50.0) -> None:
|
|
135
|
+
self._arm = arm
|
|
136
|
+
self.opening: float = float(opening)
|
|
137
|
+
self.max_opening: float = 100.0
|
|
138
|
+
self.goal_position: float = float(opening)
|
|
139
|
+
self.cancel_count: int = 0
|
|
140
|
+
self._is_on: bool = True
|
|
141
|
+
|
|
142
|
+
def is_on(self) -> bool:
|
|
143
|
+
return self._is_on
|
|
144
|
+
|
|
145
|
+
def turn_on(self) -> bool:
|
|
146
|
+
self._is_on = True
|
|
147
|
+
return True
|
|
148
|
+
|
|
149
|
+
def turn_off(self) -> bool:
|
|
150
|
+
self._is_on = False
|
|
151
|
+
return True
|
|
152
|
+
|
|
153
|
+
def get_current_opening(self) -> float:
|
|
154
|
+
return self.opening
|
|
155
|
+
|
|
156
|
+
def set_opening(self, percentage: float) -> None:
|
|
157
|
+
if not 0.0 <= float(percentage) <= 100.0:
|
|
158
|
+
raise ValueError("opening percentage must be between 0 and 100")
|
|
159
|
+
target = float(percentage)
|
|
160
|
+
if not self._is_on:
|
|
161
|
+
# Match reachy2-sdk ParallelGripper.set_opening when the gripper is off.
|
|
162
|
+
raise RuntimeError("Gripper is off. Opening value not sent.")
|
|
163
|
+
self.goal_position = target
|
|
164
|
+
|
|
165
|
+
closing = target < self.opening
|
|
166
|
+
if not closing:
|
|
167
|
+
# Release any held object when opening beyond its width.
|
|
168
|
+
for obj in self._arm._scene_objects():
|
|
169
|
+
if obj.held_by == self._arm._hand and target > obj.opening_pct:
|
|
170
|
+
obj.held_by = None
|
|
171
|
+
self.opening = target
|
|
172
|
+
return
|
|
173
|
+
|
|
174
|
+
ee_pos = self._arm.forward_kinematics(
|
|
175
|
+
self._arm.get_current_positions(degrees=False), degrees=False
|
|
176
|
+
)[:3, 3]
|
|
177
|
+
for obj in self._arm._scene_objects():
|
|
178
|
+
dist = float(np.linalg.norm(ee_pos - np.asarray(obj.position, dtype=np.float64)))
|
|
179
|
+
if dist <= obj.capture_radius_m and target <= obj.opening_pct:
|
|
180
|
+
if obj.slip:
|
|
181
|
+
# Hold the fingers at obj.opening_pct for slip_after_steps
|
|
182
|
+
# squeezes, then slip out to the commanded target.
|
|
183
|
+
obj.held_by = None
|
|
184
|
+
stalls = getattr(obj, "_stall_steps", 0)
|
|
185
|
+
if self.opening <= obj.opening_pct + 0.5 and target < obj.opening_pct - 1.0:
|
|
186
|
+
stalls += 1
|
|
187
|
+
obj._stall_steps = stalls # type: ignore[attr-defined]
|
|
188
|
+
if stalls < obj.slip_after_steps:
|
|
189
|
+
self.opening = obj.opening_pct
|
|
190
|
+
else:
|
|
191
|
+
self.opening = target
|
|
192
|
+
return
|
|
193
|
+
obj.held_by = self._arm._hand
|
|
194
|
+
self.opening = obj.opening_pct
|
|
195
|
+
return
|
|
196
|
+
|
|
197
|
+
self.opening = target
|
|
198
|
+
|
|
199
|
+
def open(self) -> None:
|
|
200
|
+
self.set_opening(100.0)
|
|
201
|
+
|
|
202
|
+
def close(self) -> None:
|
|
203
|
+
self.set_opening(0.0)
|
|
204
|
+
|
|
205
|
+
def cancel_all_goto(self) -> None:
|
|
206
|
+
self.cancel_count += 1
|
|
207
|
+
self.goal_position = self.opening
|
|
208
|
+
|
|
209
|
+
|
|
210
|
+
class FakeArm:
|
|
211
|
+
"""In-memory 7-DoF stand-in for ``reachy2_sdk.parts.arm.Arm``."""
|
|
212
|
+
|
|
213
|
+
def __init__(
|
|
214
|
+
self,
|
|
215
|
+
hand: str,
|
|
216
|
+
*,
|
|
217
|
+
scene: dict[str, FakeGraspableObject] | None = None,
|
|
218
|
+
initial_positions_rad: Sequence[float] = _DEFAULT_Q_RAD,
|
|
219
|
+
) -> None:
|
|
220
|
+
self._hand = hand
|
|
221
|
+
self._scene = scene if scene is not None else {}
|
|
222
|
+
self._shoulder_y = 0.20 if hand == "left" else -0.20
|
|
223
|
+
self._shoulder_z = 0.40
|
|
224
|
+
self._is_on = True
|
|
225
|
+
self.cancel_count = 0
|
|
226
|
+
|
|
227
|
+
q_deg = [math.degrees(float(a)) for a in initial_positions_rad]
|
|
228
|
+
self._joints = [FakeOrbitaJoint(deg) for deg in q_deg]
|
|
229
|
+
self.shoulder = FakeShoulder(self._joints[0], self._joints[1])
|
|
230
|
+
self.elbow = FakeElbow(self._joints[2], self._joints[3])
|
|
231
|
+
self.wrist = FakeWrist(self._joints[4], self._joints[5], self._joints[6])
|
|
232
|
+
self.gripper = FakeGripper(self)
|
|
233
|
+
|
|
234
|
+
def _scene_objects(self) -> list[FakeGraspableObject]:
|
|
235
|
+
return list(self._scene.values())
|
|
236
|
+
|
|
237
|
+
def is_on(self) -> bool:
|
|
238
|
+
return self._is_on
|
|
239
|
+
|
|
240
|
+
def turn_on(self) -> bool:
|
|
241
|
+
self._is_on = True
|
|
242
|
+
self.gripper.turn_on()
|
|
243
|
+
return True
|
|
244
|
+
|
|
245
|
+
def turn_off(self) -> bool:
|
|
246
|
+
self._is_on = False
|
|
247
|
+
self.gripper.turn_off()
|
|
248
|
+
return True
|
|
249
|
+
|
|
250
|
+
def get_current_positions(self, degrees: bool = True) -> list[float]:
|
|
251
|
+
if degrees:
|
|
252
|
+
return [joint.present_position for joint in self._joints]
|
|
253
|
+
return [math.radians(joint.present_position) for joint in self._joints]
|
|
254
|
+
|
|
255
|
+
def send_goal_positions(self, check_positions: bool = True) -> None:
|
|
256
|
+
del check_positions
|
|
257
|
+
if not self._is_on:
|
|
258
|
+
return
|
|
259
|
+
for joint in self._joints:
|
|
260
|
+
joint.present_position = float(joint.goal_position)
|
|
261
|
+
ee_pos = self.forward_kinematics(self.get_current_positions(degrees=False), degrees=False)[
|
|
262
|
+
:3, 3
|
|
263
|
+
]
|
|
264
|
+
for obj in self._scene.values():
|
|
265
|
+
if obj.held_by == self._hand:
|
|
266
|
+
obj.position = (float(ee_pos[0]), float(ee_pos[1]), float(ee_pos[2]))
|
|
267
|
+
|
|
268
|
+
def cancel_all_goto(self) -> None:
|
|
269
|
+
self.cancel_count += 1
|
|
270
|
+
for joint in self._joints:
|
|
271
|
+
joint.goal_position = joint.present_position
|
|
272
|
+
|
|
273
|
+
def forward_kinematics(
|
|
274
|
+
self, joints_positions: Sequence[float] | None = None, degrees: bool = True
|
|
275
|
+
) -> np.ndarray:
|
|
276
|
+
if joints_positions is None:
|
|
277
|
+
q_rad = self.get_current_positions(degrees=False)
|
|
278
|
+
else:
|
|
279
|
+
if len(joints_positions) != 7:
|
|
280
|
+
raise ValueError(f"expected 7 joint angles, got {len(joints_positions)}")
|
|
281
|
+
q_rad = [math.radians(float(v)) if degrees else float(v) for v in joints_positions]
|
|
282
|
+
|
|
283
|
+
mat = _trans(0.0, self._shoulder_y, self._shoulder_z)
|
|
284
|
+
mat = mat @ _rot_y(q_rad[0])
|
|
285
|
+
mat = mat @ _rot_x(q_rad[1])
|
|
286
|
+
mat = mat @ _rot_z(q_rad[2])
|
|
287
|
+
mat = mat @ _trans(0.0, 0.0, -_UPPER_ARM_M)
|
|
288
|
+
mat = mat @ _rot_y(q_rad[3])
|
|
289
|
+
mat = mat @ _trans(0.0, 0.0, -_FOREARM_M)
|
|
290
|
+
mat = mat @ _rot_x(q_rad[4])
|
|
291
|
+
mat = mat @ _rot_y(q_rad[5])
|
|
292
|
+
mat = mat @ _rot_z(q_rad[6])
|
|
293
|
+
mat = mat @ _trans(0.0, 0.0, -_TOOL_M)
|
|
294
|
+
return mat
|
|
295
|
+
|
|
296
|
+
def inverse_kinematics(
|
|
297
|
+
self,
|
|
298
|
+
target: np.ndarray,
|
|
299
|
+
q0: Sequence[float] | None = None,
|
|
300
|
+
degrees: bool = True,
|
|
301
|
+
) -> list[float]:
|
|
302
|
+
target_mat = np.asarray(target, dtype=np.float64)
|
|
303
|
+
if target_mat.shape != (4, 4):
|
|
304
|
+
raise ValueError(f"expected 4x4 target matrix, got {target_mat.shape}")
|
|
305
|
+
|
|
306
|
+
if q0 is None:
|
|
307
|
+
q = np.array(self.get_current_positions(degrees=False), dtype=np.float64)
|
|
308
|
+
else:
|
|
309
|
+
if len(q0) != 7:
|
|
310
|
+
raise ValueError(f"expected 7 initial joint angles, got {len(q0)}")
|
|
311
|
+
q = np.array(
|
|
312
|
+
[math.radians(float(v)) if degrees else float(v) for v in q0],
|
|
313
|
+
dtype=np.float64,
|
|
314
|
+
)
|
|
315
|
+
|
|
316
|
+
eps = 1e-6
|
|
317
|
+
damping = 1e-3
|
|
318
|
+
target_pose = matrix_to_pose(target_mat, "world")
|
|
319
|
+
|
|
320
|
+
for _ in range(80):
|
|
321
|
+
cur = self.forward_kinematics(q, degrees=False)
|
|
322
|
+
pos_err_vec = target_mat[:3, 3] - cur[:3, 3]
|
|
323
|
+
ori_err_vec = _orientation_error_vector(cur[:3, :3], target_mat[:3, :3])
|
|
324
|
+
err = np.concatenate([pos_err_vec, ori_err_vec])
|
|
325
|
+
if (
|
|
326
|
+
float(np.linalg.norm(pos_err_vec)) < 1e-5
|
|
327
|
+
and float(np.linalg.norm(ori_err_vec)) < 1e-4
|
|
328
|
+
):
|
|
329
|
+
break
|
|
330
|
+
|
|
331
|
+
jac = np.zeros((6, 7), dtype=np.float64)
|
|
332
|
+
for idx in range(7):
|
|
333
|
+
q_pert = q.copy()
|
|
334
|
+
q_pert[idx] += eps
|
|
335
|
+
pert = self.forward_kinematics(q_pert, degrees=False)
|
|
336
|
+
dp = (pert[:3, 3] - cur[:3, 3]) / eps
|
|
337
|
+
dr = _orientation_error_vector(cur[:3, :3], pert[:3, :3]) / eps
|
|
338
|
+
jac[:3, idx] = dp
|
|
339
|
+
jac[3:, idx] = dr
|
|
340
|
+
|
|
341
|
+
lhs = jac @ jac.T + (damping * damping) * np.eye(6)
|
|
342
|
+
dq = jac.T @ np.linalg.solve(lhs, err)
|
|
343
|
+
step_norm = float(np.linalg.norm(dq))
|
|
344
|
+
if step_norm > 0.25:
|
|
345
|
+
dq *= 0.25 / step_norm
|
|
346
|
+
q = q + dq
|
|
347
|
+
q = np.clip(q, -math.pi, math.pi)
|
|
348
|
+
|
|
349
|
+
reached = matrix_to_pose(self.forward_kinematics(q, degrees=False), "world")
|
|
350
|
+
pos_err, ori_err = pose_error(reached, target_pose)
|
|
351
|
+
if pos_err > 1e-3 or ori_err > 1e-2:
|
|
352
|
+
raise ValueError("no joint solution reaches the target")
|
|
353
|
+
|
|
354
|
+
if degrees:
|
|
355
|
+
return [math.degrees(float(v)) for v in q]
|
|
356
|
+
return [float(v) for v in q]
|
|
357
|
+
|
|
358
|
+
|
|
359
|
+
class FakeReachySDK:
|
|
360
|
+
"""Offline test double matching the subset of ``ReachySDK`` used by ``Reachy2Backend``."""
|
|
361
|
+
|
|
362
|
+
def __init__(self, *, has_left: bool = True, has_right: bool = True) -> None:
|
|
363
|
+
self.objects: dict[str, FakeGraspableObject] = {}
|
|
364
|
+
self.l_arm: FakeArm | None = FakeArm("left", scene=self.objects) if has_left else None
|
|
365
|
+
self.r_arm: FakeArm | None = FakeArm("right", scene=self.objects) if has_right else None
|
|
366
|
+
self._connected = True
|
|
367
|
+
|
|
368
|
+
def place_object(
|
|
369
|
+
self,
|
|
370
|
+
name: str,
|
|
371
|
+
position: tuple[float, float, float],
|
|
372
|
+
*,
|
|
373
|
+
opening_pct: float = 35.0,
|
|
374
|
+
slip: bool = False,
|
|
375
|
+
capture_radius_m: float = 0.05,
|
|
376
|
+
) -> FakeGraspableObject:
|
|
377
|
+
"""Register an object in the fake scene for grasp and slip tests."""
|
|
378
|
+
obj = FakeGraspableObject(
|
|
379
|
+
name=name,
|
|
380
|
+
position=position,
|
|
381
|
+
opening_pct=opening_pct,
|
|
382
|
+
slip=slip,
|
|
383
|
+
capture_radius_m=capture_radius_m,
|
|
384
|
+
)
|
|
385
|
+
self.objects[name] = obj
|
|
386
|
+
return obj
|
|
387
|
+
|
|
388
|
+
def is_connected(self) -> bool:
|
|
389
|
+
return self._connected
|
|
390
|
+
|
|
391
|
+
def disconnect(self) -> None:
|
|
392
|
+
self._connected = False
|
|
393
|
+
|
|
394
|
+
def turn_on(self) -> bool:
|
|
395
|
+
ok = True
|
|
396
|
+
for arm in (self.l_arm, self.r_arm):
|
|
397
|
+
if arm is not None:
|
|
398
|
+
ok = arm.turn_on() and ok
|
|
399
|
+
return ok
|
|
400
|
+
|
|
401
|
+
def turn_off(self) -> bool:
|
|
402
|
+
ok = True
|
|
403
|
+
for arm in (self.l_arm, self.r_arm):
|
|
404
|
+
if arm is not None:
|
|
405
|
+
ok = arm.turn_off() and ok
|
|
406
|
+
return ok
|
|
@@ -0,0 +1,186 @@
|
|
|
1
|
+
"""Joint-space trajectory planner for Pollen Robotics Reachy 2 arms."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
import math
|
|
6
|
+
from collections.abc import Mapping
|
|
7
|
+
from typing import Any
|
|
8
|
+
|
|
9
|
+
import numpy as np
|
|
10
|
+
|
|
11
|
+
from a2a_robots import FailureCode, Pose
|
|
12
|
+
from a2a_robots.planning import JointTrajectory, PlanError, Planner, interpolate
|
|
13
|
+
|
|
14
|
+
ARM_JOINTS: tuple[str, ...] = (
|
|
15
|
+
"shoulder_pitch",
|
|
16
|
+
"shoulder_roll",
|
|
17
|
+
"elbow_yaw",
|
|
18
|
+
"elbow_pitch",
|
|
19
|
+
"wrist_roll",
|
|
20
|
+
"wrist_pitch",
|
|
21
|
+
"wrist_yaw",
|
|
22
|
+
)
|
|
23
|
+
|
|
24
|
+
|
|
25
|
+
def hand_joints(hand: str) -> tuple[str, ...]:
|
|
26
|
+
"""Return the canonical ordered 7-DoF joint names for a Reachy 2 arm."""
|
|
27
|
+
return tuple(f"{hand}_{joint}" for joint in ARM_JOINTS)
|
|
28
|
+
|
|
29
|
+
|
|
30
|
+
def pose_to_matrix(pose: Pose) -> np.ndarray:
|
|
31
|
+
"""Convert an ``a2a_robots.Pose`` into a 4x4 homogeneous transform."""
|
|
32
|
+
w, x, y, z = (float(v) for v in pose.orientation)
|
|
33
|
+
norm = math.sqrt(w * w + x * x + y * y + z * z)
|
|
34
|
+
if norm == 0.0:
|
|
35
|
+
raise ValueError("quaternion cannot be zero")
|
|
36
|
+
w, x, y, z = w / norm, x / norm, y / norm, z / norm
|
|
37
|
+
|
|
38
|
+
xx, yy, zz = x * x, y * y, z * z
|
|
39
|
+
xy, xz, yz = x * y, x * z, y * z
|
|
40
|
+
wx, wy, wz = w * x, w * y, w * z
|
|
41
|
+
|
|
42
|
+
mat = np.eye(4, dtype=np.float64)
|
|
43
|
+
mat[0, 0] = 1.0 - 2.0 * (yy + zz)
|
|
44
|
+
mat[0, 1] = 2.0 * (xy - wz)
|
|
45
|
+
mat[0, 2] = 2.0 * (xz + wy)
|
|
46
|
+
mat[1, 0] = 2.0 * (xy + wz)
|
|
47
|
+
mat[1, 1] = 1.0 - 2.0 * (xx + zz)
|
|
48
|
+
mat[1, 2] = 2.0 * (yz - wx)
|
|
49
|
+
mat[2, 0] = 2.0 * (xz - wy)
|
|
50
|
+
mat[2, 1] = 2.0 * (yz + wx)
|
|
51
|
+
mat[2, 2] = 1.0 - 2.0 * (xx + yy)
|
|
52
|
+
mat[:3, 3] = [float(v) for v in pose.position]
|
|
53
|
+
return mat
|
|
54
|
+
|
|
55
|
+
|
|
56
|
+
def _rotation_matrix_to_quaternion(rot: np.ndarray) -> tuple[float, float, float, float]:
|
|
57
|
+
"""Convert a 3x3 rotation matrix to a canonical unit quaternion (w, x, y, z)."""
|
|
58
|
+
trace = float(rot[0, 0] + rot[1, 1] + rot[2, 2])
|
|
59
|
+
if trace > 0.0:
|
|
60
|
+
s = math.sqrt(trace + 1.0) * 2.0
|
|
61
|
+
w = 0.25 * s
|
|
62
|
+
x = (float(rot[2, 1]) - float(rot[1, 2])) / s
|
|
63
|
+
y = (float(rot[0, 2]) - float(rot[2, 0])) / s
|
|
64
|
+
z = (float(rot[1, 0]) - float(rot[0, 1])) / s
|
|
65
|
+
elif rot[0, 0] > rot[1, 1] and rot[0, 0] > rot[2, 2]:
|
|
66
|
+
s = math.sqrt(1.0 + float(rot[0, 0]) - float(rot[1, 1]) - float(rot[2, 2])) * 2.0
|
|
67
|
+
w = (float(rot[2, 1]) - float(rot[1, 2])) / s
|
|
68
|
+
x = 0.25 * s
|
|
69
|
+
y = (float(rot[0, 1]) + float(rot[1, 0])) / s
|
|
70
|
+
z = (float(rot[0, 2]) + float(rot[2, 0])) / s
|
|
71
|
+
elif rot[1, 1] > rot[2, 2]:
|
|
72
|
+
s = math.sqrt(1.0 + float(rot[1, 1]) - float(rot[0, 0]) - float(rot[2, 2])) * 2.0
|
|
73
|
+
w = (float(rot[0, 2]) - float(rot[2, 0])) / s
|
|
74
|
+
x = (float(rot[0, 1]) + float(rot[1, 0])) / s
|
|
75
|
+
y = 0.25 * s
|
|
76
|
+
z = (float(rot[1, 2]) + float(rot[2, 1])) / s
|
|
77
|
+
else:
|
|
78
|
+
s = math.sqrt(1.0 + float(rot[2, 2]) - float(rot[0, 0]) - float(rot[1, 1])) * 2.0
|
|
79
|
+
w = (float(rot[1, 0]) - float(rot[0, 1])) / s
|
|
80
|
+
x = (float(rot[0, 2]) + float(rot[2, 0])) / s
|
|
81
|
+
y = (float(rot[1, 2]) + float(rot[2, 1])) / s
|
|
82
|
+
z = 0.25 * s
|
|
83
|
+
|
|
84
|
+
norm = math.sqrt(w * w + x * x + y * y + z * z)
|
|
85
|
+
w, x, y, z = w / norm, x / norm, y / norm, z / norm
|
|
86
|
+
if w < 0.0:
|
|
87
|
+
w, x, y, z = -w, -x, -y, -z
|
|
88
|
+
return (round(w, 6), round(x, 6), round(y, 6), round(z, 6))
|
|
89
|
+
|
|
90
|
+
|
|
91
|
+
def matrix_to_pose(matrix: np.ndarray, frame: str) -> Pose:
|
|
92
|
+
"""Convert a 4x4 homogeneous transform into an ``a2a_robots.Pose``."""
|
|
93
|
+
mat = np.asarray(matrix, dtype=np.float64)
|
|
94
|
+
if mat.shape != (4, 4):
|
|
95
|
+
raise ValueError(f"expected a 4x4 transform matrix, got shape {mat.shape}")
|
|
96
|
+
pos = (round(float(mat[0, 3]), 6), round(float(mat[1, 3]), 6), round(float(mat[2, 3]), 6))
|
|
97
|
+
quat = _rotation_matrix_to_quaternion(mat[:3, :3])
|
|
98
|
+
return Pose(frame, pos, quat)
|
|
99
|
+
|
|
100
|
+
|
|
101
|
+
def pose_error(a: Pose, b: Pose) -> tuple[float, float]:
|
|
102
|
+
"""Return ``(position_error_m, orientation_error_rad)`` between two poses."""
|
|
103
|
+
dx = float(a.position[0]) - float(b.position[0])
|
|
104
|
+
dy = float(a.position[1]) - float(b.position[1])
|
|
105
|
+
dz = float(a.position[2]) - float(b.position[2])
|
|
106
|
+
pos_err = math.sqrt(dx * dx + dy * dy + dz * dz)
|
|
107
|
+
|
|
108
|
+
dot = sum(float(x) * float(y) for x, y in zip(a.orientation, b.orientation, strict=True))
|
|
109
|
+
dot = min(1.0, abs(dot))
|
|
110
|
+
ori_err = 2.0 * math.acos(dot)
|
|
111
|
+
return pos_err, ori_err
|
|
112
|
+
|
|
113
|
+
|
|
114
|
+
class Reachy2Planner(Planner):
|
|
115
|
+
"""Joint-space planner backed by a Reachy 2 arm's kinematics model."""
|
|
116
|
+
|
|
117
|
+
def __init__(
|
|
118
|
+
self,
|
|
119
|
+
arms: Mapping[str, Any],
|
|
120
|
+
*,
|
|
121
|
+
frame: str = "world",
|
|
122
|
+
max_speed_rad_s: float = 0.25,
|
|
123
|
+
max_joint_delta_rad: float = 1.0,
|
|
124
|
+
position_tolerance_m: float = 0.005,
|
|
125
|
+
orientation_tolerance_rad: float = 0.05,
|
|
126
|
+
) -> None:
|
|
127
|
+
"""``max_joint_delta_rad`` rejects IK solutions that jump farther than this
|
|
128
|
+
on any joint from the start configuration. Adapter Cartesian step limits do
|
|
129
|
+
not bound joint travel, so without this an elbow flip or wrist wrap for a
|
|
130
|
+
small pose step would stream a long arc with ``check_positions=False``."""
|
|
131
|
+
if max_speed_rad_s <= 0.0:
|
|
132
|
+
raise ValueError("max_speed_rad_s must be positive")
|
|
133
|
+
if max_joint_delta_rad <= 0.0:
|
|
134
|
+
raise ValueError("max_joint_delta_rad must be positive")
|
|
135
|
+
self._arms = dict(arms)
|
|
136
|
+
self._frame = frame
|
|
137
|
+
self._max_speed_rad_s = max_speed_rad_s
|
|
138
|
+
self._max_joint_delta_rad = max_joint_delta_rad
|
|
139
|
+
self._position_tolerance_m = position_tolerance_m
|
|
140
|
+
self._orientation_tolerance_rad = orientation_tolerance_rad
|
|
141
|
+
|
|
142
|
+
def plan(
|
|
143
|
+
self,
|
|
144
|
+
hand: str,
|
|
145
|
+
start: Mapping[str, float],
|
|
146
|
+
target: Pose,
|
|
147
|
+
dt: float,
|
|
148
|
+
) -> JointTrajectory:
|
|
149
|
+
arm = self._arms.get(hand)
|
|
150
|
+
if arm is None:
|
|
151
|
+
raise PlanError(FailureCode.UNREACHABLE, f"unknown hand {hand!r}")
|
|
152
|
+
|
|
153
|
+
joints = hand_joints(hand)
|
|
154
|
+
q0 = [float(start[name]) for name in joints]
|
|
155
|
+
target_matrix = pose_to_matrix(target)
|
|
156
|
+
|
|
157
|
+
try:
|
|
158
|
+
solution = arm.inverse_kinematics(target_matrix, q0=q0, degrees=False)
|
|
159
|
+
goal = [float(value) for value in solution]
|
|
160
|
+
if len(goal) != len(joints):
|
|
161
|
+
raise PlanError(
|
|
162
|
+
FailureCode.UNREACHABLE, "inverse kinematics returned wrong joint count"
|
|
163
|
+
)
|
|
164
|
+
# Always pass explicit 7-element joint angles in radians because
|
|
165
|
+
# reachy2-sdk 1.0.15 has a bug when forward_kinematics() reads present state.
|
|
166
|
+
fk_matrix = np.asarray(arm.forward_kinematics(goal, degrees=False), dtype=np.float64)
|
|
167
|
+
except (ValueError, RuntimeError) as exc:
|
|
168
|
+
raise PlanError(
|
|
169
|
+
FailureCode.UNREACHABLE, "no joint solution reaches the target"
|
|
170
|
+
) from exc
|
|
171
|
+
|
|
172
|
+
reached = matrix_to_pose(fk_matrix, target.frame or self._frame)
|
|
173
|
+
pos_err, ori_err = pose_error(reached, target)
|
|
174
|
+
if pos_err > self._position_tolerance_m or ori_err > self._orientation_tolerance_rad:
|
|
175
|
+
raise PlanError(FailureCode.UNREACHABLE, "no joint solution reaches the target")
|
|
176
|
+
|
|
177
|
+
for start_q, goal_q in zip(q0, goal, strict=True):
|
|
178
|
+
if abs(goal_q - start_q) > self._max_joint_delta_rad:
|
|
179
|
+
raise PlanError(
|
|
180
|
+
FailureCode.UNREACHABLE,
|
|
181
|
+
"joint solution requires too large a joint step",
|
|
182
|
+
)
|
|
183
|
+
|
|
184
|
+
speeds = [self._max_speed_rad_s] * len(joints)
|
|
185
|
+
points = interpolate(q0, goal, speeds, dt)
|
|
186
|
+
return JointTrajectory(joints, tuple(points), dt)
|
|
@@ -0,0 +1,79 @@
|
|
|
1
|
+
Metadata-Version: 2.5
|
|
2
|
+
Name: a2a-robots-pollen
|
|
3
|
+
Version: 0.1.1
|
|
4
|
+
Summary: Pollen Robotics Reachy 2 backend for a2a-robots
|
|
5
|
+
Project-URL: Repository, https://github.com/hybroai/a2a-robots
|
|
6
|
+
Author-email: HYBRO AI <info@hybro.ai>
|
|
7
|
+
License-Expression: Apache-2.0
|
|
8
|
+
Classifier: Development Status :: 2 - Pre-Alpha
|
|
9
|
+
Classifier: Programming Language :: Python :: 3
|
|
10
|
+
Classifier: Programming Language :: Python :: 3.11
|
|
11
|
+
Classifier: Programming Language :: Python :: 3.12
|
|
12
|
+
Classifier: Programming Language :: Python :: 3.13
|
|
13
|
+
Classifier: Topic :: Scientific/Engineering :: Artificial Intelligence
|
|
14
|
+
Requires-Python: >=3.11
|
|
15
|
+
Requires-Dist: a2a-robots
|
|
16
|
+
Requires-Dist: numpy>=1.26
|
|
17
|
+
Provides-Extra: dev
|
|
18
|
+
Requires-Dist: pytest-asyncio>=0.23; extra == 'dev'
|
|
19
|
+
Requires-Dist: pytest>=8; extra == 'dev'
|
|
20
|
+
Provides-Extra: sdk
|
|
21
|
+
Requires-Dist: reachy2-sdk<2,>=1.0.15; extra == 'sdk'
|
|
22
|
+
Description-Content-Type: text/markdown
|
|
23
|
+
|
|
24
|
+
# a2a-robots-pollen
|
|
25
|
+
|
|
26
|
+
[Pollen Robotics Reachy 2](https://github.com/pollen-robotics/reachy2-sdk) backend for [a2a-robots](../../README.md).
|
|
27
|
+
|
|
28
|
+
Install from PyPI: `pip install a2a-robots-pollen`.
|
|
29
|
+
|
|
30
|
+
## Reachy 2 backend
|
|
31
|
+
|
|
32
|
+
`Reachy2Backend` (aliased as `PollenBackend`) exposes Reachy 2's dual 7-DoF arms (`l_arm`, `r_arm`) and parallel grippers through `a2a_robots.RobotBackend` and `a2a_robots.RobotAdapter`.
|
|
33
|
+
|
|
34
|
+
- Arm poses come from Reachy 2 forward kinematics in the robot's own coordinate system. The backend labels them with the configured `frame` (default `"world"`); on a mobile base that is still the robot frame, not a fixed room frame, so size the workspace box accordingly. Explicit joint positions from `arm.get_current_positions(degrees=False)` are passed into `arm.forward_kinematics(q_rad, degrees=False)` to avoid the `forward_kinematics(None, degrees=False)` degree-vs-radian and 8-actuator bug in `reachy2-sdk 1.0.15`.
|
|
35
|
+
- The default planner is `Reachy2Planner`, which calls `arm.inverse_kinematics(..., degrees=False)`, verifies forward-kinematics reachability within tolerance, rejects solutions whose per-joint travel exceeds `max_joint_delta_rad` (default 1.0 rad), and generates speed-limited minimum-jerk joint trajectories streamed via `arm.send_goal_positions(check_positions=False)`. Pass `planner=` to plug in another `a2a_robots.planning.Planner`.
|
|
36
|
+
- `open` and `close` stream `arm.gripper.set_opening(...)` in small increments and check the `stop` flag every control step (20 ms by default). Stalls during the first `startup_grace_s` (0.2 s) of `close` are ignored only while the opening is still at its start value (mid-range start plus brief latency). After that grace, or once the opening has moved, stalls count — including a re-close that is already on an object. If the gripper then stays stalled short of its target (`closed_opening_pct < opening < open_opening_pct`) for `grasp_settle_s` (0.2 s), `observe()` reports `held=True` and non-empty `contact` (`contact["finger"]` is a proxy estimate `(100 - opening) / 10`, since Reachy 2's `ParallelGripper` does not measure contact force). A stall that lasts at least `contact_stall_s` (0.1 s) but then closes past the object returns `FailureCode.SLIPPED`; shorter stalls, such as a late state update, are ignored. The gripper only reports its opening, so a lag lasting between the two durations that then catches up in empty air also reads as `slipped`.
|
|
37
|
+
- `held` stays true only while the measured opening remains within `grasp_opening_tol_pct` (5%) of the opening recorded when `close` settled. `hold()` cancels active SDK gotos (`cancel_all_goto()`) on every arm and gripper and locks the current joint angles. Hands that are still near that grasp opening are re-commanded to a closed gripper target (0%) so a stop while carrying does not open the squeeze; otherwise `hold()` keeps the measured opening (so a stopped mid-`open` does not slam shut).
|
|
38
|
+
|
|
39
|
+
## Usage
|
|
40
|
+
|
|
41
|
+
`a2a-robots-pollen` is an independent `uv` project with its own `uv.lock` and an optional `sdk` extra for `reachy2-sdk`. Offline tests and `FakeReachySDK` work out of the box without installing `reachy2-sdk`:
|
|
42
|
+
|
|
43
|
+
```bash
|
|
44
|
+
# Offline with the built-in test double (no reachy2-sdk):
|
|
45
|
+
REACHY2_FAKE=1 uv run --directory packages/a2a-robots-pollen python examples/serve_reachy2.py
|
|
46
|
+
|
|
47
|
+
# Live / simulated Reachy 2 (installs the `sdk` extra: reachy2-sdk):
|
|
48
|
+
uv run --directory packages/a2a-robots-pollen --extra sdk python examples/serve_reachy2.py
|
|
49
|
+
```
|
|
50
|
+
|
|
51
|
+
The offline fake path is what unit tests exercise by default. Integration coverage against the real `reachy2-sdk` classes (in-process gRPC over `FakeReachySDK`) runs with `--extra sdk`. A smoke test against the `pollenrobotics/reachy2` Docker simulator is available when `REACHY2_HOST` is set (see Tests).
|
|
52
|
+
|
|
53
|
+
Or in Python:
|
|
54
|
+
|
|
55
|
+
```python
|
|
56
|
+
from a2a_robots import RobotAdapter, SafetyLimits, Workspace, serve_robot
|
|
57
|
+
from a2a_robots_pollen import Reachy2Backend, connect
|
|
58
|
+
|
|
59
|
+
backend = Reachy2Backend(connect("localhost"))
|
|
60
|
+
limits = SafetyLimits(workspace=Workspace("world", (-0.5, -0.8, -0.5), (0.8, 0.8, 0.8)))
|
|
61
|
+
adapter = RobotAdapter(backend, limits, name="Reachy 2")
|
|
62
|
+
serve_robot(adapter, port=9100)
|
|
63
|
+
```
|
|
64
|
+
|
|
65
|
+
## Tests
|
|
66
|
+
|
|
67
|
+
```bash
|
|
68
|
+
# Offline unit & adapter tests against FakeReachySDK
|
|
69
|
+
uv run --directory packages/a2a-robots-pollen --extra dev pytest
|
|
70
|
+
|
|
71
|
+
# Integration tests against the real reachy2-sdk classes over an in-process gRPC server
|
|
72
|
+
A2A_ROBOTS_REQUIRE_REACHY2_SDK=1 uv run --directory packages/a2a-robots-pollen \
|
|
73
|
+
--extra sdk --extra dev pytest tests/test_reachy2_sdk_grpc.py
|
|
74
|
+
|
|
75
|
+
# Smoke test against a running Pollen Reachy 2 Docker simulator (pollenrobotics/reachy2)
|
|
76
|
+
REACHY2_HOST=localhost A2A_ROBOTS_REQUIRE_REACHY2_SDK=1 uv run --directory packages/a2a-robots-pollen \
|
|
77
|
+
--extra sdk --extra dev pytest tests/test_reachy2_sdk_grpc.py -k docker_sim
|
|
78
|
+
```
|
|
79
|
+
|
|
@@ -0,0 +1,7 @@
|
|
|
1
|
+
a2a_robots_pollen/__init__.py,sha256=FBAs6eZ8Z07wSxnQZeMRIbE4KFIc9QcEtYMfl8d84fA,590
|
|
2
|
+
a2a_robots_pollen/backend.py,sha256=mOKAu2ony_BUBJZzovVTG0sm_4DawF4tM1lk25TLvFs,18535
|
|
3
|
+
a2a_robots_pollen/fake.py,sha256=_cX3mUvxTI2qGsbviGPkKuUSVV2pPzsF5NhAsmCs8KQ,13688
|
|
4
|
+
a2a_robots_pollen/planner.py,sha256=GTyjc4xvXMexJjVZCVZYWUciEaOCQaoji81cqiFfvX8,7416
|
|
5
|
+
a2a_robots_pollen-0.1.1.dist-info/METADATA,sha256=nJTTRKXd7UZ9dnTtXnquLZGkCFC0mq08ExLA_xuz0gM,5686
|
|
6
|
+
a2a_robots_pollen-0.1.1.dist-info/WHEEL,sha256=2iZrJkDIkeDh0k9SikKMqFuRP-ua6mb4Ir1AsER4BEw,87
|
|
7
|
+
a2a_robots_pollen-0.1.1.dist-info/RECORD,,
|