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,,
@@ -0,0 +1,4 @@
1
+ Wheel-Version: 1.0
2
+ Generator: hatchling 1.33.0
3
+ Root-Is-Purelib: true
4
+ Tag: py3-none-any