griip-sdk-dev 0.4.0.dev24238__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.
Files changed (195) hide show
  1. griip_sdk/__init__.py +77 -0
  2. griip_sdk/calibration/__init__.py +0 -0
  3. griip_sdk/calibration/app.py +638 -0
  4. griip_sdk/calibration/apriltag_pose.py +101 -0
  5. griip_sdk/calibration/camera_factory.py +105 -0
  6. griip_sdk/calibration/cartesian_pose.py +220 -0
  7. griip_sdk/calibration/config_store.py +131 -0
  8. griip_sdk/calibration/hand_eye_calib/README.md +109 -0
  9. griip_sdk/calibration/hand_eye_calib/__init__.py +40 -0
  10. griip_sdk/calibration/hand_eye_calib/config/routine.yaml +7 -0
  11. griip_sdk/calibration/hand_eye_calib/core.py +692 -0
  12. griip_sdk/calibration/hand_eye_calib/main.py +596 -0
  13. griip_sdk/calibration/hand_eye_calib/planning.py +122 -0
  14. griip_sdk/calibration/hand_eye_calib/routines.py +223 -0
  15. griip_sdk/calibration/hand_eye_calib/run_hand_eye_calibration.py +131 -0
  16. griip_sdk/calibration/hand_eye_calib/transforms.py +57 -0
  17. griip_sdk/calibration/mqtt_publisher.py +98 -0
  18. griip_sdk/calibration/mqtt_topics.py +15 -0
  19. griip_sdk/calibration/routines.py +36 -0
  20. griip_sdk/calibration/scanning/__init__.py +11 -0
  21. griip_sdk/calibration/scanning/environment_scanning.py +381 -0
  22. griip_sdk/calibration/scanning/run_environment_scanning.py +142 -0
  23. griip_sdk/calibration/tests/__init__.py +0 -0
  24. griip_sdk/calibration/tests/conftest.py +33 -0
  25. griip_sdk/calibration/tests/test_calibration_app.py +822 -0
  26. griip_sdk/calibration/tests/test_camera_factory.py +126 -0
  27. griip_sdk/calibration/tests/test_environment_scanning.py +547 -0
  28. griip_sdk/calibration/tests/test_hand_eye_calibration_service.py +524 -0
  29. griip_sdk/calibration/tests/test_hand_eye_helpers.py +211 -0
  30. griip_sdk/calibration/tests/test_transforms.py +126 -0
  31. griip_sdk/calibration/types.py +21 -0
  32. griip_sdk/calibration/units.py +104 -0
  33. griip_sdk/cell/__init__.py +22 -0
  34. griip_sdk/cell/builders.py +631 -0
  35. griip_sdk/cell/config.py +505 -0
  36. griip_sdk/cell/end_effector_collision_model.py +160 -0
  37. griip_sdk/cell/initialize.py +282 -0
  38. griip_sdk/cell/loader.py +102 -0
  39. griip_sdk/cell/mock_builders.py +114 -0
  40. griip_sdk/cell/parts.py +63 -0
  41. griip_sdk/cell/recipes.py +872 -0
  42. griip_sdk/cell/tests/__init__.py +0 -0
  43. griip_sdk/cell/tests/conftest.py +7 -0
  44. griip_sdk/cell/tests/test_builders.py +13 -0
  45. griip_sdk/cell/tests/test_end_effector_collision_model.py +118 -0
  46. griip_sdk/cell/tests/test_initialize.py +458 -0
  47. griip_sdk/cell/tests/test_loader.py +70 -0
  48. griip_sdk/cell/tests/test_mock_cell_smoke.py +54 -0
  49. griip_sdk/cell/tests/test_parts.py +55 -0
  50. griip_sdk/cell/tests/test_recipes.py +156 -0
  51. griip_sdk/cell/tests/test_recording_config.py +113 -0
  52. griip_sdk/errors.py +16 -0
  53. griip_sdk/grpc_adapters/__init__.py +0 -0
  54. griip_sdk/grpc_adapters/servicer_as_stub.py +166 -0
  55. griip_sdk/hardware/__init__.py +0 -0
  56. griip_sdk/hardware/bin_shaker.py +86 -0
  57. griip_sdk/hardware/camera.py +300 -0
  58. griip_sdk/hardware/fanuc_robot_client.py +266 -0
  59. griip_sdk/hardware/ft_sensor.py +352 -0
  60. griip_sdk/hardware/gripper.py +339 -0
  61. griip_sdk/hardware/grpc_vision_backend.py +123 -0
  62. griip_sdk/hardware/io_client.py +194 -0
  63. griip_sdk/hardware/mmai_vision_backend.py +265 -0
  64. griip_sdk/hardware/mocks/__init__.py +0 -0
  65. griip_sdk/hardware/mocks/camera_mock.py +62 -0
  66. griip_sdk/hardware/mocks/gripper_client_mock.py +135 -0
  67. griip_sdk/hardware/mocks/io_client_mock.py +28 -0
  68. griip_sdk/hardware/mocks/robot_client_mock.py +584 -0
  69. griip_sdk/hardware/onrobot_3fg15_gripper.py +103 -0
  70. griip_sdk/hardware/robot_client.py +1692 -0
  71. griip_sdk/hardware/robotiq_2f140_gripper.py +327 -0
  72. griip_sdk/hardware/sim_gripper.py +365 -0
  73. griip_sdk/hardware/sim_hand_eye.py +138 -0
  74. griip_sdk/hardware/sim_robot_client.py +189 -0
  75. griip_sdk/hardware/suction_gripper.py +112 -0
  76. griip_sdk/hardware/tests/__init__.py +0 -0
  77. griip_sdk/hardware/tests/conftest.py +8 -0
  78. griip_sdk/hardware/tests/test_call_with_timeout.py +56 -0
  79. griip_sdk/hardware/tests/test_command_state_fanout.py +131 -0
  80. griip_sdk/hardware/tests/test_gripper_command_listeners.py +90 -0
  81. griip_sdk/hardware/tests/test_grpc_vision_backend.py +220 -0
  82. griip_sdk/hardware/tests/test_mmai_vision_backend.py +312 -0
  83. griip_sdk/hardware/tests/test_reset_to_normal.py +101 -0
  84. griip_sdk/hardware/tests/test_robot_client_base.py +48 -0
  85. griip_sdk/hardware/tests/test_robot_client_lifecycle.py +323 -0
  86. griip_sdk/hardware/tests/test_robot_feedback_fanout.py +154 -0
  87. griip_sdk/hardware/tests/test_robotiq_2f140_gripper.py +86 -0
  88. griip_sdk/hardware/tests/test_robotiq_gripper_bus_lock.py +78 -0
  89. griip_sdk/hardware/tests/test_sim_gripper.py +230 -0
  90. griip_sdk/hardware/tests/test_sim_hand_eye.py +226 -0
  91. griip_sdk/hardware/tests/test_sim_robot_client.py +196 -0
  92. griip_sdk/hardware/tests/test_suction_gripper.py +82 -0
  93. griip_sdk/hardware/tests/test_tower_light.py +179 -0
  94. griip_sdk/hardware/tests/test_vision_codec.py +73 -0
  95. griip_sdk/hardware/tests/test_vrmcs_tcp_guard.py +81 -0
  96. griip_sdk/hardware/torquing_client.py +23 -0
  97. griip_sdk/hardware/tower_light.py +84 -0
  98. griip_sdk/hardware/ur_robot_client.py +88 -0
  99. griip_sdk/hardware/vision_codec.py +77 -0
  100. griip_sdk/log_utils.py +42 -0
  101. griip_sdk/modeling/__init__.py +0 -0
  102. griip_sdk/modeling/sphere_generation_tool/README.md +187 -0
  103. griip_sdk/modeling/sphere_generation_tool/__init__.py +8 -0
  104. griip_sdk/modeling/sphere_generation_tool/__main__.py +242 -0
  105. griip_sdk/modeling/sphere_generation_tool/_actors.py +278 -0
  106. griip_sdk/modeling/sphere_generation_tool/_geometry.py +55 -0
  107. griip_sdk/modeling/sphere_generation_tool/editor.py +1142 -0
  108. griip_sdk/modeling/sphere_generation_tool/object_io.py +53 -0
  109. griip_sdk/modeling/sphere_generation_tool/urdf_io.py +315 -0
  110. griip_sdk/modeling/tests/__init__.py +0 -0
  111. griip_sdk/modeling/tests/conftest.py +123 -0
  112. griip_sdk/modeling/tests/test_editor_math.py +80 -0
  113. griip_sdk/modeling/tests/test_editor_smoke.py +471 -0
  114. griip_sdk/modeling/tests/test_mesh_loading.py +66 -0
  115. griip_sdk/modeling/tests/test_object_io.py +72 -0
  116. griip_sdk/modeling/tests/test_urdf_io.py +367 -0
  117. griip_sdk/orchestration/__init__.py +129 -0
  118. griip_sdk/orchestration/bin_cuboid_extraction.py +77 -0
  119. griip_sdk/orchestration/bin_picking_manager.py +580 -0
  120. griip_sdk/orchestration/bin_state.py +85 -0
  121. griip_sdk/orchestration/callbacks.py +60 -0
  122. griip_sdk/orchestration/cycle_pause_gate.py +92 -0
  123. griip_sdk/orchestration/errors.py +27 -0
  124. griip_sdk/orchestration/execution/__init__.py +18 -0
  125. griip_sdk/orchestration/execution/grasp_executor.py +105 -0
  126. griip_sdk/orchestration/execution/parallel_jaw_executor.py +132 -0
  127. griip_sdk/orchestration/execution/suction_executor.py +53 -0
  128. griip_sdk/orchestration/execution/tests/__init__.py +0 -0
  129. griip_sdk/orchestration/execution/tests/test_parallel_jaw_executor.py +164 -0
  130. griip_sdk/orchestration/execution/three_finger_executor.py +144 -0
  131. griip_sdk/orchestration/grasp_provider.py +258 -0
  132. griip_sdk/orchestration/grasp_queue.py +107 -0
  133. griip_sdk/orchestration/part_setup.py +52 -0
  134. griip_sdk/orchestration/perception_inputs.py +187 -0
  135. griip_sdk/orchestration/pick_and_place_manager.py +1514 -0
  136. griip_sdk/orchestration/pick_generator.py +230 -0
  137. griip_sdk/orchestration/placement_planner.py +365 -0
  138. griip_sdk/orchestration/planning.py +59 -0
  139. griip_sdk/orchestration/proto_conversions.py +113 -0
  140. griip_sdk/orchestration/roi_scheduler.py +150 -0
  141. griip_sdk/orchestration/scoop_executor.py +122 -0
  142. griip_sdk/orchestration/tests/__init__.py +0 -0
  143. griip_sdk/orchestration/tests/conftest.py +156 -0
  144. griip_sdk/orchestration/tests/test_bin_picking_manager.py +485 -0
  145. griip_sdk/orchestration/tests/test_bin_state.py +160 -0
  146. griip_sdk/orchestration/tests/test_cycle_pause_gate.py +130 -0
  147. griip_sdk/orchestration/tests/test_grasp_provider.py +307 -0
  148. griip_sdk/orchestration/tests/test_part_setup.py +69 -0
  149. griip_sdk/orchestration/tests/test_perception_inputs.py +122 -0
  150. griip_sdk/orchestration/tests/test_phase_collision_spheres.py +57 -0
  151. griip_sdk/orchestration/tests/test_pick_and_place_manager.py +760 -0
  152. griip_sdk/orchestration/tests/test_pick_generator.py +130 -0
  153. griip_sdk/orchestration/tests/test_placement_planner.py +333 -0
  154. griip_sdk/orchestration/tests/test_planning.py +64 -0
  155. griip_sdk/orchestration/tests/test_proto_conversions.py +96 -0
  156. griip_sdk/orchestration/tests/test_scoop_executor.py +106 -0
  157. griip_sdk/orchestration/trajectory_store.py +170 -0
  158. griip_sdk/orchestration/types.py +466 -0
  159. griip_sdk/prototyping/__init__.py +0 -0
  160. griip_sdk/prototyping/config/__init__.py +20 -0
  161. griip_sdk/prototyping/config/registry.py +22 -0
  162. griip_sdk/prototyping/in_process_api.py +59 -0
  163. griip_sdk/prototyping/read_tcp.py +67 -0
  164. griip_sdk/prototyping/run_environment_scanning.py +99 -0
  165. griip_sdk/prototyping/run_hand_eye_calibration.py +104 -0
  166. griip_sdk/prototyping/run_linear_shuttle.py +556 -0
  167. griip_sdk/recording/__init__.py +71 -0
  168. griip_sdk/recording/foxglove_events.py +553 -0
  169. griip_sdk/recording/foxglove_joint_states.py +115 -0
  170. griip_sdk/recording/gripper_state_logger.py +203 -0
  171. griip_sdk/recording/gripper_state_mcap_recording.py +69 -0
  172. griip_sdk/recording/h264_video_logger.py +128 -0
  173. griip_sdk/recording/h264_video_mcap_recording.py +217 -0
  174. griip_sdk/recording/kpi_logger.py +186 -0
  175. griip_sdk/recording/kpi_mcap_recording.py +143 -0
  176. griip_sdk/recording/kpi_timer.py +148 -0
  177. griip_sdk/recording/mcap_recording.py +209 -0
  178. griip_sdk/recording/memory_utils.py +19 -0
  179. griip_sdk/recording/robot_state_logger.py +133 -0
  180. griip_sdk/recording/rotating_mcap_writer.py +326 -0
  181. griip_sdk/recording/tests/test_foxglove_event_publisher.py +324 -0
  182. griip_sdk/recording/tests/test_foxglove_joint_states.py +26 -0
  183. griip_sdk/recording/tests/test_kpi_timer.py +346 -0
  184. griip_sdk/recording/tests/test_mcap_foxglove_event_publisher.py +355 -0
  185. griip_sdk/recording/tests/test_mcap_gripper_state_logger.py +334 -0
  186. griip_sdk/recording/tests/test_mcap_h264_video_logger.py +118 -0
  187. griip_sdk/recording/tests/test_mcap_kpi_logger.py +72 -0
  188. griip_sdk/recording/tests/test_mcap_robot_state_logger.py +253 -0
  189. griip_sdk/recording/tests/test_session.py +87 -0
  190. griip_sdk/tests/__init__.py +0 -0
  191. griip_sdk/tests/test_log_utils.py +81 -0
  192. griip_sdk/tests/test_servicer_as_stub.py +69 -0
  193. griip_sdk_dev-0.4.0.dev24238.dist-info/METADATA +513 -0
  194. griip_sdk_dev-0.4.0.dev24238.dist-info/RECORD +195 -0
  195. griip_sdk_dev-0.4.0.dev24238.dist-info/WHEEL +4 -0
@@ -0,0 +1,638 @@
1
+ """High-level recipe for assembling a calibration app from a cell YAML.
2
+
3
+ Parallel to :mod:`griip_sdk.cell.recipes`: ``build_calibration_app`` wires a
4
+ calibration workflow the way ``build_pick_only_manager`` wires picking. The
5
+ returned :class:`CalibrationApp` drives hand-eye calibration and the
6
+ operator-driven environment scan. See the README for the consumer walkthrough.
7
+
8
+ Protos: requests use ``griip_core.generated.griip_pb2``; the mmai-griip-api
9
+ server serves the schema-identical ``griip.proto``. The two are kept in
10
+ lockstep -- if they diverge, gRPC calls silently misbehave.
11
+ """
12
+
13
+ from __future__ import annotations
14
+
15
+ import logging
16
+ from contextlib import contextmanager
17
+ from datetime import datetime
18
+ from pathlib import Path
19
+ from typing import Any, Callable, Iterator, cast
20
+
21
+ import grpc
22
+ import numpy as np
23
+ import numpy.typing as npt
24
+ from spatialmath import SE3, SO3
25
+ from vention.vision.v1.vision_service_abc import ImageServiceBackend
26
+
27
+ from griip_sdk.calibration.camera_factory import build_calibration_camera
28
+ from griip_sdk.calibration.config_store import (
29
+ DEFAULT_CALIBRATION_DATA_PATH,
30
+ CalibrationConfigStore,
31
+ utc_ts,
32
+ )
33
+ from griip_sdk.calibration.hand_eye_calib import (
34
+ CalibrationCancelledError,
35
+ HandEyeCalibrationService,
36
+ )
37
+ from griip_sdk.calibration.hand_eye_calib.planning import (
38
+ build_and_configure_hand_eye_planning_service,
39
+ resolve_hand_eye_planner_params,
40
+ )
41
+ from griip_sdk.calibration.hand_eye_calib.transforms import (
42
+ compute_camera_to_tcp_transform,
43
+ load_camera_to_flange_transform,
44
+ )
45
+ from griip_sdk.calibration.mqtt_publisher import MqttProgressPublisher, compose_on_progress
46
+ from griip_sdk.calibration.mqtt_topics import ENVIRONMENT_PROGRESS_TOPIC, HAND_EYE_PROGRESS_TOPIC
47
+ from griip_sdk.calibration.routines import DEFAULT_ENCIRCLE_ROUTINE
48
+ from griip_sdk.calibration.scanning import EnvironmentScanningService
49
+ from griip_sdk.calibration.types import PerceptionStub, PlanningStub
50
+ from griip_sdk.cell import load_cell_config
51
+ from griip_sdk.cell.builders import resolve_camera_calibration
52
+ from griip_sdk.cell.config import PrototypeCellConfig
53
+ from griip_sdk.cell.initialize import build_robot_initialize_request, send_initialize
54
+ from griip_sdk.errors import ConfigError
55
+ from griip_core.generated import griip_pb2 as pb
56
+ from griip_core.generated import griip_pb2_grpc
57
+ from griip_sdk.grpc_adapters.servicer_as_stub import ServicerAsStub
58
+ from griip_sdk.hardware.robot_client import VrmcsRobotClient, build_vrmcs_client
59
+
60
+ logger = logging.getLogger(__name__)
61
+
62
+
63
+ class CalibrationApp:
64
+ """Returned by :func:`build_calibration_app`; exposes the calibration verbs.
65
+
66
+ Owns shared infrastructure (robot client, started camera client, planning +
67
+ perception stubs, calibration store) so repeated verb calls don't re-pay
68
+ setup. Per-verb services are built on demand.
69
+ """
70
+
71
+ def __init__(
72
+ self,
73
+ *,
74
+ cell: PrototypeCellConfig,
75
+ config_path: Path,
76
+ robot_client: VrmcsRobotClient,
77
+ vision_backend: ImageServiceBackend,
78
+ cam_name: str,
79
+ camera_mxid: str,
80
+ sensor: str,
81
+ store: CalibrationConfigStore,
82
+ calib_data_root: Path | str,
83
+ planning_stub: PlanningStub,
84
+ perception_stub: PerceptionStub,
85
+ secondary_source_id: str = "right",
86
+ mqtt_publisher: MqttProgressPublisher | None = None,
87
+ strict_intrinsics: bool = True,
88
+ ) -> None:
89
+ self._cell = cell
90
+ self._config_path = config_path
91
+ self._robot_client = robot_client
92
+ self._vision_backend = vision_backend
93
+ self._cam_name = cam_name
94
+ self._camera_mxid = camera_mxid
95
+ self._sensor = sensor
96
+ self._secondary_source_id = secondary_source_id
97
+ self._store = store
98
+ self._calib_data_root = calib_data_root
99
+ self._planning_stub = planning_stub
100
+ self._perception_stub = perception_stub
101
+ self._mqtt_publisher = mqtt_publisher
102
+ self._strict_intrinsics = strict_intrinsics
103
+
104
+ self._env_session_service: EnvironmentScanningService | None = None
105
+ self._env_session_sequence_dir: Path | None = None
106
+ self._env_session_next_index: int = 0
107
+ self._env_session_previous_tcp_offset: SE3 | None = None
108
+ self._reset_env_session_state()
109
+
110
+ # ----- Hand-eye calibration --------------------------------------
111
+
112
+ def run_hand_eye_calibration(
113
+ self,
114
+ *,
115
+ on_progress: Callable[[int, str, str], None] | None = None,
116
+ is_cancelled: Callable[[], bool] | None = None,
117
+ save_dir: Path | None = None,
118
+ routine: dict[str, Any] | None = None,
119
+ bootstrap_path: Path | None = None,
120
+ ) -> SE3:
121
+ """Plan to ``cell.positions.calibration_joints``, run the sphere routine, then return there.
122
+
123
+ Raises :class:`ConfigError` if ``calibration_joints`` is unset. Persists
124
+ the result under ``X_cam_flg__{cam_name}__{mxid}__{sensor}``. Emits exactly
125
+ one terminal ``on_progress`` tick (``success`` 100 / ``error`` 0 /
126
+ ``cancelled`` 0), even on early failures, so consumers needn't wrap the call.
127
+
128
+ Args:
129
+ on_progress: ``(percent, status, message)`` callback.
130
+ is_cancelled: predicate polled between waypoints to stop cooperatively.
131
+ save_dir: output dir for artifacts; defaults to a timestamped ``/tmp`` dir.
132
+ routine: sphere-routine dict overriding
133
+ :data:`~griip_sdk.calibration.routines.DEFAULT_ENCIRCLE_ROUTINE`.
134
+ bootstrap_path: pre-computed bootstrap result to skip the bootstrap leg.
135
+ """
136
+ # Compose the consumer's callback with the SDK's own MQTT publisher (if a
137
+ # broker was configured) so the terminal ticks below also reach the UI.
138
+ mqtt_progress = self._mqtt_publisher.make_on_progress(HAND_EYE_PROGRESS_TOPIC) if self._mqtt_publisher else None
139
+ effective_on_progress = compose_on_progress(on_progress, mqtt_progress)
140
+ try:
141
+ service = self._build_hand_eye_service(
142
+ save_dir=save_dir,
143
+ routine=routine,
144
+ bootstrap_path=bootstrap_path,
145
+ )
146
+ x_cam_flg: SE3 = service.calibrate(on_progress=effective_on_progress, is_cancelled=is_cancelled)
147
+ config_name = self._hand_eye_name()
148
+ self._store.add_new_version(config_name, {"X_cam_flg": x_cam_flg.A.tolist()})
149
+ logger.info("Saved %s under %s", config_name, self._store.root)
150
+ except CalibrationCancelledError:
151
+ self._emit_terminal(effective_on_progress, 0, "cancelled", "Hand-eye calibration cancelled")
152
+ raise
153
+ except Exception as exc:
154
+ self._emit_terminal(effective_on_progress, 0, "error", f"Hand-eye calibration failed: {exc}")
155
+ raise
156
+ else:
157
+ self._emit_terminal(effective_on_progress, 100, "success", "Hand-eye calibration complete")
158
+ return x_cam_flg
159
+
160
+ @staticmethod
161
+ def _emit_terminal(
162
+ on_progress: Callable[[int, str, str], None] | None,
163
+ percent: int,
164
+ status: str,
165
+ message: str,
166
+ ) -> None:
167
+ """Emit one guarded terminal ``on_progress`` tick (no-op if unset)."""
168
+ if on_progress is not None:
169
+ on_progress(percent, status, message)
170
+
171
+ def last_hand_eye_calibration_timestamp(self) -> datetime | None:
172
+ return self._store.timestamp_of(self._hand_eye_name())
173
+
174
+ # ----- Environment-scan session ----------------------------------
175
+
176
+ def start_environment_session(self, save_dir: Path | None = None) -> Path:
177
+ """Open a new operator-driven environment-scan session.
178
+
179
+ Builds the :class:`EnvironmentScanningService`, creates the sequence
180
+ directory, persists post-crop intrinsics, and switches the robot into
181
+ freedrive. Raises ``RuntimeError`` if a session is already open; returns
182
+ the sequence directory.
183
+ """
184
+ if self._env_session_service is not None:
185
+ raise RuntimeError(
186
+ "An environment-scan session is already open; call end_environment_session() first.",
187
+ )
188
+
189
+ x_tcp_flg = self._env_scan_tcp_offset_transform()
190
+ previous_tcp_offset = self._robot_client.X_tcp_flg
191
+ self._robot_client.set_tcp_offset(x_tcp_flg)
192
+
193
+ try:
194
+ service = self._build_env_scan_service(save_dir=save_dir, x_tcp_flg=x_tcp_flg)
195
+ sequence_dir = service.start_session()
196
+ next_index = service.resume_index(sequence_dir)
197
+ except Exception:
198
+ # Restore the frame mutated above so a failed start can't strand the
199
+ # robot on the env-scan offset -- and so a retry doesn't capture the
200
+ # mutated offset as its "previous" and later restore to the wrong frame.
201
+ self._robot_client.set_tcp_offset(previous_tcp_offset)
202
+ raise
203
+
204
+ # Commit session state only once the start fully succeeded.
205
+ self._env_session_previous_tcp_offset = previous_tcp_offset
206
+ self._env_session_service = service
207
+ self._env_session_sequence_dir = sequence_dir
208
+ self._env_session_next_index = next_index
209
+ self._publish_env_progress(
210
+ "in_progress",
211
+ f"Session started: {sequence_dir.name}",
212
+ extra={"sessionName": sequence_dir.name, "photoCount": self._env_session_next_index},
213
+ )
214
+ return sequence_dir
215
+
216
+ def take_environment_image(self) -> int:
217
+ """Capture one frame in the current session and return its index.
218
+
219
+ Reads the TCP pose, grabs stereo + depth, and writes the per-frame files
220
+ under the active sequence directory (robot stays in freedrive between
221
+ captures). Raises ``RuntimeError`` if no session is open.
222
+ """
223
+ service, sequence_dir = self._require_open_session()
224
+ frame_index = self._env_session_next_index
225
+ service.capture_frame(sequence_dir, frame_index)
226
+ self._env_session_next_index += 1
227
+ self._publish_env_progress(
228
+ "in_progress",
229
+ f"Captured photo {self._env_session_next_index}",
230
+ extra={
231
+ "sessionName": sequence_dir.name,
232
+ "photoCount": self._env_session_next_index,
233
+ },
234
+ )
235
+ return frame_index
236
+
237
+ def end_environment_session(self) -> Path:
238
+ """Close the active session, restore the robot, and persist a store entry
239
+ pointing at the sequence directory.
240
+
241
+ Returns the sequence directory. Raises ``RuntimeError`` if no session is open.
242
+ """
243
+ service, sequence_dir = self._require_open_session()
244
+ photo_count = self._env_session_next_index
245
+ try:
246
+ service.end_session()
247
+ finally:
248
+ # Restore the TCP offset mutated at session start (so a later
249
+ # hand-eye run plans from the same frame), then clear session state.
250
+ # Both must run even if end_session() raises, or a failed close
251
+ # leaves a zombie service the next end_environment_session() would
252
+ # re-close. Restore before reset: the reset clears the saved offset.
253
+ if self._env_session_previous_tcp_offset is not None:
254
+ self._robot_client.set_tcp_offset(self._env_session_previous_tcp_offset)
255
+ self._reset_env_session_state()
256
+ config_name = self._env_scan_name()
257
+ self._store.add_new_version(config_name, {"sequence_dir": str(sequence_dir)})
258
+ logger.info("Saved %s under %s", config_name, self._store.root)
259
+ self._publish_env_progress(
260
+ "success",
261
+ f"Session ended: {photo_count} photo(s) saved",
262
+ extra={"sessionName": None, "photoCount": photo_count},
263
+ )
264
+ return sequence_dir
265
+
266
+ def _publish_env_progress(self, status: str, message: str, *, extra: dict[str, Any]) -> None:
267
+ """Publish an environment-scan progress event to MQTT (no-op if unconfigured)."""
268
+ if self._mqtt_publisher is None:
269
+ return
270
+ payload = {"status": status, "message": message, **extra}
271
+ self._mqtt_publisher.publish(ENVIRONMENT_PROGRESS_TOPIC, payload)
272
+
273
+ def last_environment_scan_timestamp(self) -> datetime | None:
274
+ return self._store.timestamp_of(self._env_scan_name())
275
+
276
+ def _reset_env_session_state(self) -> None:
277
+ """Clear the operator-driven environment-scan session state."""
278
+ self._env_session_service = None
279
+ self._env_session_sequence_dir = None
280
+ self._env_session_next_index = 0
281
+ self._env_session_previous_tcp_offset = None
282
+
283
+ def _require_open_session(self) -> tuple[EnvironmentScanningService, Path]:
284
+ """Return the open session's ``(service, sequence_dir)`` or raise ``RuntimeError``."""
285
+ if self._env_session_service is None or self._env_session_sequence_dir is None:
286
+ raise RuntimeError(
287
+ "No environment-scan session open; call start_environment_session() first.",
288
+ )
289
+ return self._env_session_service, self._env_session_sequence_dir
290
+
291
+ # ----- Name builders ----------------------------------------------
292
+
293
+ def _hand_eye_name(self) -> str:
294
+ return CalibrationConfigStore.format_config_name(
295
+ "X_cam_flg",
296
+ self._cam_name,
297
+ self._camera_mxid,
298
+ self._sensor,
299
+ )
300
+
301
+ def _env_scan_name(self) -> str:
302
+ return CalibrationConfigStore.format_config_name("env_scan", self._cam_name, self._camera_mxid)
303
+
304
+ # ----- Service builders -------------------------------------------
305
+
306
+ def _resolve_calibration(self) -> tuple[npt.NDArray[np.float64], npt.NDArray[np.float64], int, int, float]:
307
+ """Resolve intrinsics/distortion/baseline for ``self._sensor`` via the vision ABC.
308
+
309
+ If the store has no calibio entry: ``strict_intrinsics=True`` (default)
310
+ raises :class:`FileNotFoundError` naming the missing config;
311
+ ``strict_intrinsics=False`` logs a warning and proceeds with whatever
312
+ calibration ``vision_backend`` currently reports -- the camera-swap
313
+ escape hatch for a fresh camera with no calibio data yet. Either way,
314
+ the backend itself owns applying calibio (see
315
+ :class:`~griip_sdk.hardware.mmai_vision_backend.MmaiVisionServiceBackend`);
316
+ this only gates the strict check and reads the result.
317
+
318
+ The calibio store gate only applies to the legacy ``mmai`` camera
319
+ backend; native gRPC vision services own their calibration, so their
320
+ reported intrinsics are authoritative as-is.
321
+
322
+ Returns ``(cam_intr, dist_coeffs, im_width, im_height, stereo_baseline)``.
323
+ """
324
+ if self._cell.camera.backend == "mmai":
325
+ intr_config_name = CalibrationConfigStore.format_config_name(
326
+ "calibio",
327
+ self._cam_name,
328
+ self._camera_mxid,
329
+ )
330
+ if not self._store.list_versions(intr_config_name):
331
+ if self._strict_intrinsics:
332
+ raise FileNotFoundError(
333
+ f"No intrinsic calibration found in store for '{intr_config_name}'. "
334
+ f"Upload calibio intrinsics under {self._store.root / intr_config_name}, or "
335
+ "construct the app with `strict_intrinsics=False` to fall back to live "
336
+ "camera defaults.",
337
+ )
338
+ logger.warning(
339
+ "No calibio intrinsics for '%s' in store; falling back to live camera defaults (strict_intrinsics=False).",
340
+ intr_config_name,
341
+ )
342
+ cam_intr, dist_coeffs, im_width, im_height, stereo_baseline, _, _ = resolve_camera_calibration(
343
+ self._vision_backend, self._sensor, self._secondary_source_id
344
+ )
345
+ return cam_intr, dist_coeffs, im_width, im_height, stereo_baseline
346
+
347
+ def _resolve_calibration_joints(self) -> list[float]:
348
+ """Return ``cell.positions.calibration_joints`` or raise :class:`ConfigError`.
349
+
350
+ The cell YAML is the single source of truth for the starting pose -- no
351
+ runtime fallback. A missing field is an operator-actionable config problem.
352
+ """
353
+ joints = self._cell.positions.calibration_joints
354
+ if joints is None:
355
+ raise ConfigError(
356
+ f"cell.positions.calibration_joints is not set in {self._config_path}. "
357
+ "Commissioning needs to author the starting pose in the cell YAML before "
358
+ "calibration can run.",
359
+ )
360
+ return list(joints)
361
+
362
+ def _resolve_save_dir(self, save_dir: Path | None, default: str) -> Path:
363
+ """Return ``save_dir`` (or a timestamped ``default``), created on disk."""
364
+ resolved = Path(save_dir) if save_dir is not None else Path(default)
365
+ resolved.mkdir(parents=True, exist_ok=True)
366
+ return resolved
367
+
368
+ def _build_hand_eye_service(
369
+ self,
370
+ *,
371
+ save_dir: Path | None,
372
+ routine: dict[str, Any] | None,
373
+ bootstrap_path: Path | None,
374
+ ) -> HandEyeCalibrationService:
375
+ cam_intr, dist_coeffs, im_width, im_height, _stereo_baseline = self._resolve_calibration()
376
+
377
+ # Bundled routine wins by default; consumers override with `routine=`.
378
+ # The legacy hand_eye_routine.yaml-next-to-cell-yaml lookup is gone.
379
+ calibration_routine = routine if routine is not None else DEFAULT_ENCIRCLE_ROUTINE
380
+
381
+ resolved_save_dir = self._resolve_save_dir(save_dir, f"/tmp/hand_eye_calibration_{utc_ts()}")
382
+
383
+ return HandEyeCalibrationService(
384
+ robo=self._robot_client,
385
+ vision_backend=self._vision_backend,
386
+ store=self._store,
387
+ planning_stub=cast(griip_pb2_grpc.PlanningServiceStub, self._planning_stub),
388
+ config=calibration_routine,
389
+ cam_intr=cam_intr,
390
+ dist_coeffs=dist_coeffs,
391
+ im_width=im_width,
392
+ im_height=im_height,
393
+ sensor_name=self._sensor,
394
+ save_dir=str(resolved_save_dir),
395
+ calibration_joint_angles=self._resolve_calibration_joints(),
396
+ bootstrap_path=str(bootstrap_path) if bootstrap_path is not None else None,
397
+ velocity_scaling=self._cell.motion.calibration.velocity_scaling,
398
+ acceleration_scaling=self._cell.motion.calibration.acceleration_scaling,
399
+ )
400
+
401
+ def _env_scan_tcp_offset_transform(self) -> SE3:
402
+ """Build the flange-to-TCP offset transform from ``cell.tool``."""
403
+ if self._cell.tool is None:
404
+ logger.warning(
405
+ "cell.tool is unset; defaulting tcp_offset to [0,0,0]. Env-scan output will be miscalibrated "
406
+ "for cells with a physically-mounted tool.",
407
+ )
408
+ tcp_offset = [0.0, 0.0, 0.0]
409
+ else:
410
+ tcp_offset = list(self._cell.tool.tcp_offset)
411
+ return SE3.Rt(R=SO3(), t=np.array(tcp_offset, dtype=np.float64))
412
+
413
+ def _build_env_scan_service(self, *, save_dir: Path | None, x_tcp_flg: SE3) -> EnvironmentScanningService:
414
+ cam_intr, _dist_coeffs, image_width, image_height, stereo_baseline = self._resolve_calibration()
415
+
416
+ x_cam_flg = load_camera_to_flange_transform(
417
+ camera_vendor=self._cam_name,
418
+ camera_mx_id=self._camera_mxid,
419
+ sensor=self._sensor,
420
+ calibration_data_path=self._calib_data_root,
421
+ )
422
+ x_cam_tcp = compute_camera_to_tcp_transform(x_cam_flg, x_tcp_flg)
423
+
424
+ resolved_save_dir = self._resolve_save_dir(save_dir, f"/data/vention/scene_scanning_dataset_{utc_ts()}")
425
+
426
+ return EnvironmentScanningService(
427
+ robot_client=self._robot_client,
428
+ vision_backend=self._vision_backend,
429
+ left_source_id=self._sensor,
430
+ right_source_id=self._secondary_source_id,
431
+ perception_stub=self._perception_stub,
432
+ camera_intrinsics=cam_intr,
433
+ x_cam_tcp=x_cam_tcp,
434
+ stereo_baseline=stereo_baseline,
435
+ save_dir=resolved_save_dir,
436
+ image_width=image_width,
437
+ image_height=image_height,
438
+ )
439
+
440
+
441
+ def _build_in_process_planning_stub(
442
+ robot_client: VrmcsRobotClient,
443
+ cell: PrototypeCellConfig,
444
+ cell_config_directory: Path,
445
+ *,
446
+ cuboids_path_override: str | None = None,
447
+ ) -> PlanningStub:
448
+ # Lazy import: pulls mmai_griip_api transitively (dev-only dep; see
449
+ # pyproject.toml). Importing it at module top-level would break wheel installs.
450
+ from mmai_griip_api.api.planning_api import PlanningApi
451
+ from mmai_griip_api.api.service_registry import ServiceRegistry
452
+
453
+ planning_service = build_and_configure_hand_eye_planning_service(
454
+ robot_client,
455
+ cell,
456
+ cell_config_directory,
457
+ cuboids_path_override=cuboids_path_override,
458
+ )
459
+ registry = ServiceRegistry()
460
+ registry.planning_service = planning_service
461
+ return ServicerAsStub(PlanningApi(registry))
462
+
463
+
464
+ def _build_in_process_perception_stub(cell: PrototypeCellConfig) -> PerceptionStub:
465
+ # Lazy import: same reasoning as _build_in_process_planning_stub.
466
+ from mmai_griip_api.api.perception_api import PerceptionApi
467
+ from mmai_griip_api.api.service_registry import ServiceRegistry
468
+ from mmai_griip_api.services.perception.foundation_stereo_client import FoundationStereoClient
469
+ from mmai_griip_api.services.perception.perception_service import PerceptionService
470
+
471
+ foundation_stereo_client = FoundationStereoClient(host=cell.perception.services.foundation_stereo)
472
+ perception_service = PerceptionService(
473
+ sam_client=None,
474
+ foundation_pose_client=None,
475
+ foundation_stereo_client=foundation_stereo_client,
476
+ )
477
+ registry = ServiceRegistry()
478
+ registry.perception_service = perception_service
479
+ return ServicerAsStub(PerceptionApi(registry))
480
+
481
+
482
+ def _build_grpc_calibration_stubs(griip_api_url: str) -> tuple[grpc.Channel, PlanningStub, PerceptionStub]:
483
+ """Open a gRPC channel to ``griip_api_url``; return ``(channel, planning_stub, perception_stub)``.
484
+
485
+ Uses the SDK's own generated stubs so callers don't need the dev-only
486
+ ``mmai_griip_api`` package. The caller closes the channel.
487
+ """
488
+ channel = grpc.insecure_channel(griip_api_url)
489
+ planning_stub = griip_pb2_grpc.PlanningServiceStub(channel)
490
+ perception_stub = griip_pb2_grpc.PerceptionServiceStub(channel)
491
+ return channel, planning_stub, perception_stub
492
+
493
+
494
+ def _initialize_remote_api(channel: grpc.Channel, cell: PrototypeCellConfig) -> None:
495
+ """Register the remote planning subsystem before ``ConfigurePlanner``.
496
+
497
+ A fresh server holds no ``PlanningService`` until ``GriipApiAdmin.Initialize``
498
+ runs; until then ``ConfigurePlanner``/``PlanPath`` abort with ``UNAVAILABLE``.
499
+ Sends a part-free, perception-mocked Initialize that builds the planner and
500
+ registers the robot with VRMCS.
501
+
502
+ Idempotent. Initialize replaces the server's service registry, so calibration
503
+ and picking sessions must not share one server concurrently.
504
+ """
505
+ admin_stub = griip_pb2_grpc.GriipApiAdminStub(channel)
506
+ send_initialize(admin_stub, build_robot_initialize_request(cell.model_dump(mode="python")))
507
+ logger.info("Remote mmai-griip-api initialized for hand-eye (robot registered; server-side perception mocked)")
508
+
509
+
510
+ def _configure_remote_planner(
511
+ planning_stub: PlanningStub,
512
+ cell: PrototypeCellConfig,
513
+ cell_config_directory: Path,
514
+ *,
515
+ cuboids_path_override: str | None = None,
516
+ ) -> None:
517
+ """Send the hand-eye :class:`pb.ConfigurePlannerRequest` over a gRPC stub.
518
+
519
+ Shares :func:`resolve_hand_eye_planner_params` with the in-process path so
520
+ both transports resolve gripper spheres (via the
521
+ :class:`~griip_sdk.cell.end_effector_collision_model.EndEffectorCollisionModel`)
522
+ and cuboids identically. The spheres are always sent inline.
523
+ """
524
+ request_kwargs = resolve_hand_eye_planner_params(cell, cell_config_directory, cuboids_path_override=cuboids_path_override)
525
+ inline_spheres = request_kwargs.pop("gripper_collision_spheres")
526
+ request_kwargs["gripper_collision_spheres"] = [
527
+ pb.CollisionSpheres(center=pb.Vector3(x=sphere[0], y=sphere[1], z=sphere[2]), radius=sphere[3]) for sphere in inline_spheres
528
+ ]
529
+
530
+ response = planning_stub.ConfigurePlanner(pb.ConfigurePlannerRequest(**request_kwargs))
531
+ logger.info(
532
+ "Hand-eye PlanningService configured over gRPC: environment_id=%s robot_dof=%s cuboids=%d",
533
+ response.environment_id,
534
+ response.robot_dof,
535
+ response.num_cuboids_loaded,
536
+ )
537
+
538
+
539
+ @contextmanager
540
+ def build_calibration_app(
541
+ cell_yaml: Path | str,
542
+ *,
543
+ calib_data: Path | str | None = None,
544
+ cuboids_path: Path | str | None = None,
545
+ mqtt_host: str | None = "localhost",
546
+ mqtt_port: int = 1883,
547
+ strict_intrinsics: bool = True,
548
+ ) -> Iterator[CalibrationApp]:
549
+ """Yield a wired :class:`CalibrationApp` for the cell at ``cell_yaml``.
550
+
551
+ Loads the config, builds the robot + camera clients, wires planning +
552
+ perception stubs (in-process for ``griip_api.mode: direct``; gRPC for
553
+ ``grpc``), and builds the store. In gRPC mode it first issues a part-free
554
+ Initialize then ``ConfigurePlanner`` (without Initialize the latter aborts
555
+ with ``UNAVAILABLE``). On exit, releases the robot registration if this client
556
+ created it (failures logged) and
557
+ closes the channel. To inject custom stubs, construct :class:`CalibrationApp`
558
+ directly -- its ``__init__`` is keyword-only.
559
+
560
+ Args:
561
+ cell_yaml: path to the cell YAML.
562
+ calib_data: store root; defaults to :data:`DEFAULT_CALIBRATION_DATA_PATH`.
563
+ cuboids_path: override for the VAMP collision cuboids; defaults to
564
+ ``cell.environment.cuboids_path``.
565
+ mqtt_host: broker host for live progress events; defaults to
566
+ ``"localhost"``. Pass ``None`` to disable publishing. Hand-eye ticks
567
+ land on ``griip/calibration/hand_eye/progress``, environment ticks on
568
+ ``griip/calibration/environment/progress``; the React components in
569
+ ``@vention/machine-apps-components`` subscribe to both.
570
+ mqtt_port: broker port; defaults to ``1883``.
571
+ strict_intrinsics: when ``True`` (default), missing calibio intrinsics
572
+ raise :class:`FileNotFoundError`; ``False`` falls back to live
573
+ ``camera_info`` (camera-swap escape hatch).
574
+ """
575
+ resolved_path = Path(cell_yaml).expanduser().resolve()
576
+ cell = load_cell_config(resolved_path)
577
+
578
+ effective_cuboids_path = cell.environment.cuboids_path
579
+ if cuboids_path is not None:
580
+ effective_cuboids_path = str(Path(cuboids_path).expanduser().resolve())
581
+ logger.info("Using cuboids_path override: %s -> %s", cell.environment.cuboids_path, effective_cuboids_path)
582
+
583
+ logger.info("=== build_calibration_app ===")
584
+ logger.info("Config: %s griip_api.mode: %s", resolved_path, cell.griip_api.mode)
585
+
586
+ robot_client = build_vrmcs_client(resolved_path, cell)
587
+ grpc_channel: grpc.Channel | None = None
588
+ mqtt_publisher: MqttProgressPublisher | None = None
589
+ if mqtt_host is not None:
590
+ mqtt_publisher = MqttProgressPublisher(host=mqtt_host, port=mqtt_port)
591
+ try:
592
+ robot_client.reset_to_normal()
593
+
594
+ sensor = cell.camera.sensor
595
+ secondary_source_id = cell.camera.right_source_id
596
+ calib_root: Path | str = calib_data if calib_data is not None else DEFAULT_CALIBRATION_DATA_PATH
597
+ store = CalibrationConfigStore(root=calib_root)
598
+
599
+ # require_calibio=False: the app gates calibio per-verb via
600
+ # strict_intrinsics (see _resolve_calibration) instead of at build time.
601
+ camera = build_calibration_camera(cell.camera, store=store, require_calibio=False)
602
+
603
+ if cell.griip_api.mode == "grpc":
604
+ grpc_channel, planning_stub, perception_stub = _build_grpc_calibration_stubs(cell.griip_api.url)
605
+ logger.info("Connected to mmai-griip-api over gRPC at %s", cell.griip_api.url)
606
+ _initialize_remote_api(grpc_channel, cell)
607
+ _configure_remote_planner(planning_stub, cell, resolved_path.parent, cuboids_path_override=effective_cuboids_path)
608
+ else:
609
+ planning_stub = _build_in_process_planning_stub(
610
+ robot_client, cell, resolved_path.parent, cuboids_path_override=effective_cuboids_path
611
+ )
612
+ perception_stub = _build_in_process_perception_stub(cell)
613
+
614
+ yield CalibrationApp(
615
+ cell=cell,
616
+ config_path=resolved_path,
617
+ robot_client=robot_client,
618
+ vision_backend=camera.vision_backend,
619
+ cam_name=camera.camera_vendor,
620
+ camera_mxid=camera.camera_mx_id,
621
+ sensor=sensor,
622
+ secondary_source_id=secondary_source_id,
623
+ store=store,
624
+ calib_data_root=calib_root,
625
+ planning_stub=planning_stub,
626
+ perception_stub=perception_stub,
627
+ mqtt_publisher=mqtt_publisher,
628
+ strict_intrinsics=strict_intrinsics,
629
+ )
630
+ finally:
631
+ try:
632
+ robot_client.release_registration()
633
+ except Exception as exc:
634
+ logger.warning("release_registration: %s", exc)
635
+ if grpc_channel is not None:
636
+ grpc_channel.close()
637
+ if mqtt_publisher is not None:
638
+ mqtt_publisher.close()