fix(omny): calibrate gripper force at init, recover from killed #FES/#FRM

A wrong force0 from #rio1con makes #POSCHK stop all programs without a
flag or error. That killed #FES, left initmv=1, and every later #FES
exited silently, reported only as "Failed to drive axis to limit".

- omny_calibrate_gripper_force() at the start of omny_init_stages and
  after XQ#CRESET; a voltage away from gripper_sensorvoltagetarget needs
  confirmation and names the config file to adjust
- clear a leftover initmv before #FES/#FRM when thread 0 is idle
- report the abort reason when a drive or reference search fails

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01M8zoDz1DGZxhmZy9jzaUYf

csaxs_bec/bec_ipython_client/plugins/omny/omny.py

@@ -18,7 +18,9 @@ from csaxs_bec.bec_ipython_client.plugins.OMNY_shared.omny_general_tools import
 from csaxs_bec.bec_ipython_client.plugins.omny.omny_optics_mixin import OMNYOpticsMixin
 from csaxs_bec.bec_ipython_client.plugins.omny.omny_rt import OMNY_rt_client
 from csaxs_bec.bec_ipython_client.plugins.omny.omny_sample_transfer_mixin import (
+    GRIPPER_SENSOR_VOLTAGE_TOLERANCE,
     OMNYSampleTransferMixin,
+    gripper_sensor_voltage_message,
 )
 from csaxs_bec.bec_ipython_client.plugins.omny.x_ray_eye_align import XrayEyeAlign
 from csaxs_bec.bec_ipython_client.plugins.OMNY_shared.tomo_queue_mixin import TomoQueueMixin
@@ -216,6 +218,46 @@ class OMNYInitStagesMixin:
         device._ogalil_folerr_reset_and_ignore()
         time.sleep(0.3)
 
+    def _omny_wait_for_gripper_rio(self, timeout=30):
+        """Wait until the RIO is connected and #rio1con and #INIT have finished."""
+        controller = dev.otransy.controller
+        start = time.time()
+        while (
+            float(controller.socket_put_and_receive("MG _IHH2")) != -2
+            or float(controller.socket_put_and_receive("MG _XQ5")) != -1
+            or float(controller.socket_put_and_receive("MG _XQ0")) != -1
+        ):
+            if time.time() - start > timeout:
+                raise OMNYInitError(
+                    "The gripper RIO (192.168.5.15) is not connected to controller 1."
+                )
+            time.sleep(0.2)
+
+    def omny_calibrate_gripper_force(self):
+        """Define the current gripper sensor voltage as zero force (force0).
+
+        #rio1con takes force0 right after the RIO connects and that value can be off.
+        The firmware then sees a force above forcablm and stops all programs as soon
+        as #POSCHK runs, without setting a flag or an error.
+        """
+        self._omny_wait_for_gripper_rio()
+        controller = dev.otransy.controller
+        voltage = float(controller.socket_put_and_receive("MG @AN[8001]"))
+        target = self.OMNYTools._get_user_param_safe("otransy", "gripper_sensorvoltagetarget")
+        if np.fabs(voltage - target) >= GRIPPER_SENSOR_VOLTAGE_TOLERANCE:
+            message = gripper_sensor_voltage_message(voltage, target)
+            self.OMNYTools.printredbold(message)
+            if not self.OMNYTools.yesno(
+                f"Is the gripper free, so that {voltage:.2f} V can be used as zero force?"
+            ):
+                raise OMNYInitError(message)
+        controller.socket_put_confirmed("XQ#GRFCAL")
+        time.sleep(0.2)
+        force0 = float(controller.socket_put_and_receive("MG force0"))
+        if np.fabs(force0 - voltage) >= GRIPPER_SENSOR_VOLTAGE_TOLERANCE:
+            raise OMNYInitError(f"Gripper force calibration failed: force0 is {force0:.2f} V.")
+        self.OMNYTools.printgreen(f"Gripper force calibrated at {force0:.2f} V.")
+
     def omny_init_stages(self, autoconfirm=0, autoretry=0):
 
         if self.OMNYTools.yesno(
@@ -225,6 +267,8 @@ class OMNYInitStagesMixin:
         else:
             return
 
+        self.omny_calibrate_gripper_force()
+
         # #maybe replace 2 by something
         # axis_id = dev.fheater._config["deviceConfig"].get("axis_Id")
         # axis_id_numeric = self.axis_id_to_numeric(axis_id)
@@ -259,6 +303,8 @@ class OMNYInitStagesMixin:
         dev.osamx.controller.socket_put_confirmed("XQ#CRESET")
         dev.osamy.controller.socket_put_confirmed("XQ#CRESET")
         time.sleep(1)
+        # the reset restarts #rio1con, which takes force0 again
+        self.omny_calibrate_gripper_force()
 
         if self.OMNYTools.yesno(
             "drive fzp to -Z,-X,+Y limit and reference?", "y", autoconfirm=autoconfirm

csaxs_bec/bec_ipython_client/plugins/omny/omny_sample_transfer_mixin.py

@@ -27,6 +27,17 @@ class OMNYTransferError(Exception):
     pass
 
 
+GRIPPER_SENSOR_VOLTAGE_TOLERANCE = 0.5
+
+
+def gripper_sensor_voltage_message(sensorvoltage: float, target: float) -> str:
+    return (
+        f"Sensorvoltage is {sensorvoltage:.2f} V, expected {target:.2f} V. This indicates a "
+        "sensor error. If the sensor offset really changed, also adjust "
+        "gripper_sensorvoltagetarget of otransy in the config file ptycho_omny.yaml."
+    )
+
+
 # otransfer_park_slot()'s oparkz targets (regular gripper transfer path).
 _OPARKZ_TARGET_POSITIONS = {
     2: -0.9 - 29 * 3 - 42 - 0.05,
@@ -924,9 +935,10 @@ class OMNYSampleTransferMixin:
         gripper_sensorvoltagetarget = self._get_user_param_safe(
             "otransy", "gripper_sensorvoltagetarget"
         )
-        if not np.fabs(sensorvoltage - gripper_sensorvoltagetarget) < 0.5:
+        deviation = np.fabs(sensorvoltage - gripper_sensorvoltagetarget)
+        if not deviation < GRIPPER_SENSOR_VOLTAGE_TOLERANCE:
             raise OMNYTransferError(
-                f"Sensorvoltage is {sensorvoltage:.2f} V. This indicates a sensor error."
+                gripper_sensor_voltage_message(sensorvoltage, gripper_sensorvoltagetarget)
             )
 
     def otransfer_get_sample(self, pin_position: int):

csaxs_bec/devices/omny/galil/ogalil_ophyd.py

@@ -17,6 +17,7 @@ from csaxs_bec.devices.omny.galil.galil_ophyd import (
     BECConfigError,
     GalilAxesReferenced,
     GalilController,
+    GalilError,
     GalilMotorIsMoving,
     GalilSetpointSignal,
     GalilSignalRO,
@@ -169,6 +170,60 @@ class OMNYGalilController(GalilController):
         print("not yet implemented")
         return 0
 
+    def _clear_stale_init_motion(self) -> None:
+        """Clear the init-motion flags left behind by a #FES/#FRM that was killed.
+
+        #FES and #FRM set initmv and clear it only on their last lines. If they are
+        killed partway (AB from the temperature, following error or gripper force
+        checks, AB/HX from #CMDERR), initmv stays 1 and every later #NEWPAR, #FES
+        and #FRM exits silently. Both routines run on thread 0, so initmv=1 with
+        thread 0 idle can only be such a leftover.
+        """
+        if float(self.socket_put_and_receive("MG initmv")) != 1 or self.is_thread_active(0):
+            return
+        logger.warning(f"Clearing stale initmv on {self.sock.host}:{self.sock.port}.")
+        for flag in ("initmv", "fesmv", "frmmv"):
+            self.socket_put_confirmed(f"{flag}=0")
+
+    def _init_motion_abort_reason(self) -> str:
+        """Describe why #FES/#FRM did not finish, as far as the controller can tell."""
+        if float(self.socket_put_and_receive("MG tempab")) == 1:
+            if float(self.socket_put_and_receive("MG timeab")) == 1:
+                return "A motor was on for too long (timeab)."
+            return "Motor over temperature (tempab)."
+        if float(self.socket_put_and_receive("MG folerr")) == 1:
+            return "Following error (folerr)."
+        if (
+            float(self.socket_put_and_receive("MG contrID")) == 1
+            and float(self.socket_put_and_receive("MG _IHH2")) == -2
+        ):
+            voltage = float(self.socket_put_and_receive("MG @AN[8001]"))
+            force0 = float(self.socket_put_and_receive("MG force0"))
+            limit = float(self.socket_put_and_receive("MG forcablm"))
+            if abs(voltage - force0) > limit:
+                return (
+                    f"Gripper force check stopped all programs: sensor {voltage:.2f} V, "
+                    f"force0 {force0:.2f} V, limit {limit:.2f} V. Check that the gripper "
+                    "is free, then run omny_calibrate_gripper_force()."
+                )
+        if float(self.socket_put_and_receive("MG initmv")) == 1:
+            return "The routine was killed before it finished, no reason found."
+        return "The controller reports no abort."
+
+    def drive_axis_to_limit(self, axis_Id_numeric: int, direction: str, verbose=0) -> None:
+        self._clear_stale_init_motion()
+        try:
+            super().drive_axis_to_limit(axis_Id_numeric, direction, verbose)
+        except GalilError as exc:
+            raise GalilError(f"{exc} {self._init_motion_abort_reason()}") from exc
+
+    def find_reference(self, axis_Id_numeric: int, verbose=0, raise_error=1) -> None:
+        self._clear_stale_init_motion()
+        try:
+            super().find_reference(axis_Id_numeric, verbose, raise_error)
+        except GalilError as exc:
+            raise GalilError(f"{exc} {self._init_motion_abort_reason()}") from exc
+
     def _ogalil_folerr_reset_and_ignore(self, axis_id_numeric: int) -> None:
         self.socket_put_confirmed(f"folaxerr[{axis_id_numeric}]=0")
         self.socket_put_confirmed("folerr=0")

csaxs_bec/devices/sim/sim_galil.py

@@ -182,6 +182,12 @@ class SimGalilState:
             "allaxref": 0.0,
             "axisref": 0.0,
             "frmmv": 0.0,
+            "fesmv": 0.0,
+            "initmv": 0.0,
+            "folerr": 0.0,
+            "force0": 0.0,
+            "forcablm": 2.0,
+            "_IHH2": -2.0,
             # dialect status variables (LamNI / OMNY); overridable via sim_variables
             "swver": 0.0,
             "allaxrer": 0.0,
@@ -196,7 +202,7 @@ class SimGalilState:
         }
         self.digital_inputs = {14: 1.0}  # light curtain reads triggered by default
         self.digital_outputs = {9: 0.0, 15: 1.0}  # gripper closed, lights on
-        self.analog_inputs = {}
+        self.analog_inputs = {8001: -2.3}  # gripper force sensor
         self._thread_deadlines = {}  # thread_id -> unix time until which thread is active
         self._transfer_thread = None
         self._transfer_abort = threading.Event()
@@ -643,6 +649,10 @@ class SimGalilSocket(SimSocketBase):
             with state.lock:
                 state.variables["grstat"] = 1.0
             return ":"
+        if routine == "#GRFCAL":
+            with state.lock:
+                state.variables["force0"] = state.analog_inputs[8001]
+            return ":"
         if routine == "#CRESET":
             with state.lock:
                 state.variables["confirm"] = 0.0

tests/tests_bec_ipython_client/test_omny_gripper_force.py

@@ -0,0 +1,73 @@
+from unittest import mock
+
+import pytest
+
+from csaxs_bec.bec_ipython_client.plugins.omny import omny as omny_module
+from csaxs_bec.bec_ipython_client.plugins.omny.omny import OMNYInitError, OMNYInitStagesMixin
+
+READY = {"MG _IHH2": "-2.0000", "MG _XQ5": "-1.0000", "MG _XQ0": "-1.0000"}
+
+
+@pytest.fixture
+def setup():
+    replies = {**READY, "MG @AN[8001]": "-2.1826", "MG force0": "-2.1826"}
+    client = OMNYInitStagesMixin()
+    client.OMNYTools = mock.MagicMock()
+    client.OMNYTools._get_user_param_safe.return_value = -2.30
+    fake_dev = mock.MagicMock()
+    fake_dev.otransy.controller.socket_put_and_receive.side_effect = lambda cmd: replies[cmd]
+    with mock.patch.object(omny_module, "dev", fake_dev, create=True):
+        with mock.patch.object(omny_module.time, "sleep"):
+            yield client, fake_dev.otransy.controller, replies
+
+
+def test_calibrates_without_asking_at_the_expected_voltage(setup):
+    client, controller, _ = setup
+    client.omny_calibrate_gripper_force()
+    controller.socket_put_confirmed.assert_called_once_with("XQ#GRFCAL")
+    client.OMNYTools.yesno.assert_not_called()
+
+
+def test_unexpected_voltage_declined_aborts_and_names_the_config(setup):
+    client, controller, replies = setup
+    replies["MG @AN[8001]"] = "1.0000"
+    client.OMNYTools.yesno.return_value = False
+    with pytest.raises(OMNYInitError, match="gripper_sensorvoltagetarget.*ptycho_omny.yaml"):
+        client.omny_calibrate_gripper_force()
+    controller.socket_put_confirmed.assert_not_called()
+
+
+def test_unexpected_voltage_accepted_calibrates_and_names_the_config(setup):
+    client, controller, replies = setup
+    replies["MG @AN[8001]"] = "1.0000"
+    replies["MG force0"] = "1.0000"
+    client.OMNYTools.yesno.return_value = True
+    client.omny_calibrate_gripper_force()
+    assert "ptycho_omny.yaml" in client.OMNYTools.printredbold.call_args.args[0]
+    controller.socket_put_confirmed.assert_called_once_with("XQ#GRFCAL")
+
+
+def test_calibration_not_taken_by_the_controller_is_an_error(setup):
+    client, _, replies = setup
+    replies["MG force0"] = "2.5488"
+    with pytest.raises(OMNYInitError, match="calibration failed"):
+        client.omny_calibrate_gripper_force()
+
+
+def test_waits_for_the_rio_thread_to_finish(setup):
+    client, controller, replies = setup
+    calls = iter(["-2.0000", "0.0000", "-2.0000", "-1.0000", "-1.0000"])
+
+    def reply(cmd):
+        return next(calls) if cmd in READY else replies[cmd]
+
+    controller.socket_put_and_receive.side_effect = reply
+    client._omny_wait_for_gripper_rio()
+    assert next(calls, None) is None
+
+
+def test_rio_not_connected_times_out(setup):
+    client, _, replies = setup
+    replies["MG _IHH2"] = "0.0000"
+    with pytest.raises(OMNYInitError, match="192.168.5.15"):
+        client._omny_wait_for_gripper_rio(timeout=0)

tests/tests_devices/test_ogalil_init_motion.py

@@ -0,0 +1,83 @@
+from unittest import mock
+
+import pytest
+
+from csaxs_bec.devices.omny.galil.galil_ophyd import GalilController, GalilError
+from csaxs_bec.devices.omny.galil.ogalil_ophyd import OMNYGalilController
+
+
+def _controller(replies: dict) -> mock.MagicMock:
+    """OMNY controller stand-in with the firmware's MG replies; thread 0 idle by default."""
+    controller = mock.MagicMock(spec=OMNYGalilController)
+    controller.sock = mock.MagicMock(host="mpc3217.psi.ch", port=8081)
+    controller.socket_put_and_receive.side_effect = lambda cmd: replies[cmd]
+    controller.is_thread_active.return_value = False
+    return controller
+
+
+def _flags_written(controller) -> list:
+    return [call.args[0] for call in controller.socket_put_confirmed.call_args_list]
+
+
+def test_stale_initmv_is_cleared_with_thread_0_idle():
+    controller = _controller({"MG initmv": "1.0000"})
+    OMNYGalilController._clear_stale_init_motion(controller)
+    assert _flags_written(controller) == ["initmv=0", "fesmv=0", "frmmv=0"]
+
+
+def test_initmv_of_a_running_routine_is_kept():
+    controller = _controller({"MG initmv": "1.0000"})
+    controller.is_thread_active.return_value = True
+    OMNYGalilController._clear_stale_init_motion(controller)
+    controller.socket_put_confirmed.assert_not_called()
+
+
+def test_clear_initmv_nothing_to_do():
+    controller = _controller({"MG initmv": "0.0000"})
+    OMNYGalilController._clear_stale_init_motion(controller)
+    controller.socket_put_confirmed.assert_not_called()
+
+
+NO_ABORT = {
+    "MG tempab": "0.0000",
+    "MG timeab": "0.0000",
+    "MG folerr": "0.0000",
+    "MG contrID": "1.0000",
+    "MG _IHH2": "-2.0000",
+    "MG @AN[8001]": "-2.1826",
+    "MG force0": "-2.1826",
+    "MG forcablm": "2.0000",
+    "MG initmv": "0.0000",
+}
+
+
+@pytest.mark.parametrize(
+    "changes, expected",
+    [
+        ({}, "no abort"),
+        ({"MG force0": "2.5488"}, "Gripper force check"),
+        ({"MG tempab": "1.0000"}, "over temperature"),
+        ({"MG tempab": "1.0000", "MG timeab": "1.0000"}, "timeab"),
+        ({"MG folerr": "1.0000"}, "Following error"),
+        ({"MG initmv": "1.0000"}, "killed"),
+        # the force check only exists on controller 1 and only with the RIO connected
+        ({"MG contrID": "2.0000", "MG force0": "2.5488"}, "no abort"),
+        ({"MG _IHH2": "0.0000", "MG force0": "2.5488"}, "no abort"),
+    ],
+)
+def test_abort_reason(changes, expected):
+    controller = _controller({**NO_ABORT, **changes})
+    assert expected in OMNYGalilController._init_motion_abort_reason(controller)
+
+
+@pytest.mark.parametrize(
+    "method, args",
+    [("drive_axis_to_limit", (4, "forward")), ("find_reference", (4,))],
+)
+def test_failure_reports_the_reason(method, args):
+    controller = _controller({})
+    controller._init_motion_abort_reason.return_value = "the reason"
+    with mock.patch.object(GalilController, method, side_effect=GalilError("Failed.")):
+        with pytest.raises(GalilError, match="Failed. the reason"):
+            getattr(OMNYGalilController, method)(controller, *args)
+    controller._clear_stale_init_motion.assert_called_once()