Add checks to grip_state before attempting HWP spinup (#824)

* Adds checks to grip_state before attempting HWP spinup

* [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci

---------

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

socs/agents/hwp_supervisor/agent.py

@@ -1117,8 +1117,18 @@ def check_acu_ok_for_spinup():
                         raise RuntimeError(f"ACU commanded elevation is {acu.el_commanded_position} deg, "
                                            f"outside of allowed range ({acu.min_el}, {acu.max_el})")
 
+            def check_gripper_ok_for_spinup():
+                if hwp_state.gripper is None:  # No gripper was specified in the config file -- ignore
+                    return
+                grip_state = hwp_state.gripper.grip_state
+                if grip_state != "ungripped":
+                    raise RuntimeError(
+                        f"Spinup attempted while grip state is: {grip_state}"
+                    )
+
             if isinstance(state, ControlState.PIDToFreq):
                 check_acu_ok_for_spinup()
+                check_gripper_ok_for_spinup()
                 self.run_and_validate(clients.pid.set_direction,
                                       kwargs={'direction': state.direction})
                 self.run_and_validate(clients.pid.declare_freq,
@@ -1213,6 +1223,7 @@ def check_acu_ok_for_spinup():
             elif isinstance(state, ControlState.ConstVolt):
                 if state.voltage > 0:
                     check_acu_ok_for_spinup()
+                    check_gripper_ok_for_spinup()
                 self.run_and_validate(clients.pmx.set_on)
                 self.run_and_validate(clients.pid.set_direction,
                                       kwargs={'direction': state.direction})

tests/integration/test_hwp_supervisor_agent_integration.py

@@ -5,6 +5,7 @@
 import pytest
 from integration.util import docker_compose_file  # noqa: F401
 from integration.util import create_crossbar_fixture
+from ocs.ocs_client import OCSReply
 from ocs.testing import _AgentRunner, create_client_fixture
 
 from socs.testing.hwp_emulator import HWPEmulator
@@ -174,6 +175,13 @@ def test_supervisor_grip(hwp_em, supervisor_agent, sup_client) -> None:
     state = get_hwp_state(sup_client)
     assert state["gripper"]["grip_state"] == "warm"
 
+    # Verify that spinning is blocked while the HWP is gripped
+    reply: OCSReply = sup_client.pid_to_freq(target_freq=2.0)
+    if reply.session['success']:
+        print("Spin up process successful though  HWP was gripped...")
+        print(reply.session)
+        assert False
+
 
 @pytest.mark.integtest
 def test_hwp_spinup(supervisor_agent, sup_client) -> None:
@@ -182,3 +190,10 @@ def test_hwp_spinup(supervisor_agent, sup_client) -> None:
     assert get_hwp_state(sup_client)["is_spinning"]
     status = sup_client.pid_to_freq.status()
     pprint(status.session["data"])
+
+    # Verify that gripping the HWP is blocked while the HWP is spinning
+    reply: OCSReply = sup_client.grip_hwp()
+    if reply.session['success']:
+        print("Grip attempted even though HWP is spinning")
+        print(reply.session)
+        assert False