From e72fecb4525bee2dffd6892e3ff8b7b0ee948a0d Mon Sep 17 00:00:00 2001 From: Jack Lashner Date: Thu, 1 Aug 2024 22:13:27 -0400 Subject: [PATCH 01/14] Adds grip and ungrip fns to hwp-supervisor and safety checks --- docs/agents/hwp_supervisor_agent.rst | 21 +++++ socs/agents/hwp_supervisor/agent.py | 129 ++++++++++++++++++++++++++- 2 files changed, 149 insertions(+), 1 deletion(-) diff --git a/docs/agents/hwp_supervisor_agent.rst b/docs/agents/hwp_supervisor_agent.rst index 56a2aee52..646214c7c 100644 --- a/docs/agents/hwp_supervisor_agent.rst +++ b/docs/agents/hwp_supervisor_agent.rst @@ -35,6 +35,8 @@ Here is an example of a config block you can add to your ocs site-config file:: '--hwp-encoder-id', 'hwp-bbb-e1', '--hwp-pmx-id', 'hwp-pmx', '--hwp-pid-id', 'hwp-pid', + '--hwp-pcu-id', 'hwp-pcu', + '--hwp-gripper-id', 'hwp-gripper', '--ups-id', 'power-ups-az', '--ups-minutes-remaining-thresh', 45, '--iboot-id', 'power-iboot-hwp-2', @@ -43,6 +45,12 @@ Here is an example of a config block you can add to your ocs site-config file:: '--driver-power-agent-type', 'synaccess', '--driver-power-cycle-twice', '--driver-power-cycle-wait-time', 300, + + '--acu-instance-id', 'acu', + '--acu-min-el', 48.0, + '--acu-max-el', 90.0, + '--acu-max-time-since-update', 30.0, + '--mount-velocity-grip-thresh', 0.005, ]} For SATP1, we use an ibootbar to power the driver, and it is not necessary to @@ -57,12 +65,20 @@ cycle the driver power twice, so the config will look like:: '--hwp-encoder-id', 'hwp-bbb-e1', '--hwp-pmx-id', 'hwp-pmx', '--hwp-pid-id', 'hwp-pid', + '--hwp-pcu-id', 'hwp-pcu', + '--hwp-gripper-id', 'hwp-gripper', '--ups-id', 'power-ups-az', '--ups-minutes-remaining-thresh', 45, '--iboot-id', 'power-iboot-hwp-2', '--driver-iboot-id', 'power-iboot-hwp-2', '--driver-iboot-outlets', 1, 2, '--driver-power-agent-type', 'iboot', + + '--acu-instance-id', 'acu', + '--acu-min-el', 48.0, + '--acu-max-el', 90.0, + '--acu-max-time-since-update', 30.0, + '--mount-velocity-grip-thresh', 0.005, ]} .. note:: @@ -140,6 +156,11 @@ Before spinning up the HWP, the agent will check that - The current elevation and commanded elevation are in the range ``(acu-min-el, acu-max-el)``. - The ACU state info has been updated within ``acu-max-time-since-update`` seconds. +Before gripping or ungripping the HWP, the agent will check that: +- The current elevation and commanded elevation are in the range ``(acu-min-el, acu-max-el)``. +- The ACU state info has been updated within ``acu-max-time-since-update`` seconds. +- The az and el velocity is less than ``mount-velocity-grip-thresh``. + Examples ``````````` Below is an example client script that runs the PID to freq operation, and waits diff --git a/socs/agents/hwp_supervisor/agent.py b/socs/agents/hwp_supervisor/agent.py index 3f3f346b5..a8e689e96 100644 --- a/socs/agents/hwp_supervisor/agent.py +++ b/socs/agents/hwp_supervisor/agent.py @@ -112,6 +112,7 @@ class HWPClients: gripper_iboot: Optional[OCSClient] = None driver_iboot: Optional[OCSClient] = None pcu: Optional[OCSClient] = None + gripper: Optional[OCSClient] = None @dataclass @@ -166,6 +167,8 @@ class ACUState: Maximum elevation allowed before restricting spin-up [deg] max_time_since_update : float Maximum time since last update before restricting spin-up[sec] + mount_velocity_grip_thresh: float + Maximum mount velocity for gripping the HWP [deg/s] . Attributes ------------ @@ -182,10 +185,13 @@ class ACUState: min_el: float max_el: float max_time_since_update: float + mount_velocity_grip_thresh: float el_current_position: Optional[float] = None el_commanded_position: Optional[float] = None el_current_velocity: Optional[float] = None + az_current_velocity: Optional[float] = None + last_updated: Optional[float] = None def update(self): @@ -200,6 +206,8 @@ def update(self): self.el_current_position = d['Elevation current position'] self.el_commanded_position = d['Elevation commanded position'] self.el_current_velocity = d['Elevation current velocity'] + self.az_current_velocity = d['Azimuth current velocity'] + t = d.get('timestamp_agent') if t is None: t = time.time() @@ -269,7 +277,8 @@ def from_args(cls, args: argparse.Namespace): instance_id=args.acu_instance_id, min_el=args.acu_min_el, max_el=args.acu_max_el, - max_time_since_update=args.acu_max_time_since_update + max_time_since_update=args.acu_max_time_since_update, + mount_velocity_grip_thresh=args.mount_velocity_grip_thresh, ) log.info("ACU state checking enabled: instance_id={id}", id=self.acu.instance_id) @@ -626,6 +635,20 @@ class PmxOff: """ success: bool = True + @dataclass + class GripHWP: + """ + Grips the HWP + """ + pass + + @dataclass + class UngripHWP: + """ + Ungrips the HWP + """ + pass + @dataclass class Abort: """Abort current action""" @@ -830,6 +853,43 @@ 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_ok_for_grip(): + """Validates telescope is in a state that is ok for gripping/ungripping""" + acu = hwp_state.acu + + if np.abs(hwp_state.pid_current_freq) > 0.02: + raise RuntimeError("Cannot grip HWP while spinning") + + if acu is not None: + if acu.last_updated is None: + raise RuntimeError( + f"No ACU data has been received from instance-id {acu.instance_id}" + ) + tdiff = time.time() - acu.last_updated + if tdiff > acu.max_time_since_update: + raise RuntimeError(f"ACU state has not been updated in {tdiff} sec") + + if not (acu.min_el <= acu.el_current_position <= acu.max_el): + raise RuntimeError( + f"ACU elevation is {acu.el_current_pos} deg, " + f"outside of allowed range ({acu.min_el}, {acu.max_el})" + ) + if not (acu.min_el <= acu.el_commanded_position <= acu.max_el): + raise RuntimeError( + f"ACU commanded elevation is {acu.el_commanded_position} deg, " + f"outside of allowed range ({acu.min_el}, {acu.max_el})" + ) + if np.abs(acu.az_current_velocity) > np.abs(acu.mount_velocity_grip_thresh): + raise RuntimeError( + f"ACU az-velocity is {acu.az_current_velocity} deg/s, " + f"above threshold of {acu.mount_velocity_grip_thresh} deg/s" + ) + if np.abs(acu.el_current_velocity) > np.abs(acu.mount_velocity_grip_thresh): + raise RuntimeError( + f"ACU el-velocity is {acu.el_current_velocity} deg/s, " + f"above threshold of {acu.mount_velocity_grip_thresh} deg/s" + ) + if isinstance(state, ControlState.PIDToFreq): check_acu_ok_for_spinup() self.run_and_validate(clients.pid.set_direction, @@ -945,6 +1005,18 @@ def check_acu_ok_for_spinup(): ) self.action.set_state(ControlState.Done(success=state.success)) + elif isinstance(state, ControlState.GripHWP): + if np.abs(hwp_state.pid_current_freq) > 0.02: + raise RuntimeError("Cannot grip HWP while spinning") + check_ok_for_grip() + self.run_and_validate(clients.gripper.grip) + self.action.set_state(ControlState.Done(success=state.success)) + + elif isinstance(state, ControlState.UngripHWP): + check_ok_for_grip() + self.run_and_validate(clients.gripper.ungrip) + self.action.set_state(ControlState.Done(success=state.success)) + elif isinstance(state, ControlState.Brake): self.run_and_validate( clients.pcu.send_command, @@ -1122,6 +1194,7 @@ def get_client(id): lakeshore=get_client(self.ybco_lakeshore_id), gripper_iboot=get_client(self.gripper_iboot_id), driver_iboot=get_client(self.driver_iboot_id), + gripper=get_client(self.args.hwp_gripper_id), ) @ocs_agent.param('test_mode', type=bool, default=False) @@ -1441,6 +1514,54 @@ def pmx_off(self, session, params): action.sleep_until_complete(session=session) return action.success, f"Completed with state: {action.cur_state_info.state}" + def grip_hwp(self, session, params): + """grip() + + **Task** - Grip the HWP if ACU and HWP state passes checks. + + Notes + -------- + + Example of ``session.data``:: + + >>> session['data'] + {'action': + {'action_id': 3, + 'completed': True, + 'cur_state': {'class': 'Done', 'msg': None, 'success': True}, + 'state_history': List[ConrolState], + 'success': True} + } + """ + state = ControlState.GripHWP() + action = self.control_state_machine.request_new_action(state) + action.sleep_until_complete(session=session) + return action.success, f"Completed with state: {action.cur_state_info.state}" + + def ungrip_hwp(self, session, params): + """ungrip() + + **Task** - Ungrip the HWP if ACU and HWP state passes checks. + + Notes + -------- + + Example of ``session.data``:: + + >>> session['data'] + {'action': + {'action_id': 3, + 'completed': True, + 'cur_state': {'class': 'Done', 'msg': None, 'success': True}, + 'state_history': List[ConrolState], + 'success': True} + } + """ + state = ControlState.UngripHWP() + action = self.control_state_machine.request_new_action(state) + action.sleep_until_complete(session=session) + return action.success, f"Completed with state: {action.cur_state_info.state}" + def abort_action(self, session, params): """abort_action() @@ -1544,6 +1665,8 @@ def make_parser(parser=None): help="Instance ID for HWP pid agent") pgroup.add_argument('--hwp-pcu-id', help="Instance ID for HWP PCU agent") + pgroup.add_argument('--hwp-gripper-id', + help="Instance ID for HWP Gripper agent") pgroup.add_argument('--ups-id', help="Instance ID for UPS agent") pgroup.add_argument('--ups-minutes-remaining-thresh', type=float, help="Threshold for UPS minutes remaining before a " @@ -1592,6 +1715,10 @@ def make_parser(parser=None): '--acu-max-time-since-update', type=float, default=30.0, help="Max amount of time since last ACU update before allowing HWP spin up", ) + pgroup.add_argument( + '--mount-vel-grip-thresh', type=float, default=0.005, + help="Max mount velocity (both az and el) for gripping the HWP" + ) pgroup.add_argument('--forward-dir', choices=['cw', 'ccw'], default="cw", help="Whether the PID 'forward' direction is cw or ccw") From 9493dd6fa80b7fff422b13320efddedcd5cb23c4 Mon Sep 17 00:00:00 2001 From: Jack Lashner Date: Fri, 2 Aug 2024 19:24:14 -0400 Subject: [PATCH 02/14] Adds check of boresight angle --- docs/agents/hwp_supervisor_agent.rst | 1 + socs/agents/hwp_supervisor/agent.py | 39 +++++++++++++++++++++++----- 2 files changed, 34 insertions(+), 6 deletions(-) diff --git a/docs/agents/hwp_supervisor_agent.rst b/docs/agents/hwp_supervisor_agent.rst index 646214c7c..5d84032cd 100644 --- a/docs/agents/hwp_supervisor_agent.rst +++ b/docs/agents/hwp_supervisor_agent.rst @@ -51,6 +51,7 @@ Here is an example of a config block you can add to your ocs site-config file:: '--acu-max-el', 90.0, '--acu-max-time-since-update', 30.0, '--mount-velocity-grip-thresh', 0.005, + '--grip-max-boresight-angle', 1.0, ]} For SATP1, we use an ibootbar to power the driver, and it is not necessary to diff --git a/socs/agents/hwp_supervisor/agent.py b/socs/agents/hwp_supervisor/agent.py index a8e689e96..2857f5a23 100644 --- a/socs/agents/hwp_supervisor/agent.py +++ b/socs/agents/hwp_supervisor/agent.py @@ -10,11 +10,11 @@ import ocs import txaio from ocs import client_http, ocs_agent, site_config -from ocs.client_http import ControlClientError +from ocs.client_http import ControlClientError, ControlClient from ocs.ocs_client import OCSClient, OCSReply from ocs.ocs_twisted import Pacemaker -client_cache = {} +client_cache: Dict[str, ControlClient] = {} def get_op_data(agent_id, op_name, log=None, test_mode=False): @@ -169,6 +169,9 @@ class ACUState: Maximum time since last update before restricting spin-up[sec] mount_velocity_grip_thresh: float Maximum mount velocity for gripping the HWP [deg/s] . + grip_max_boresight_angle: float + Maximum absolute boresight angle [deg] for which operating the grippers + is allowed. Attributes ------------ @@ -178,6 +181,12 @@ class ACUState: Commanded el position [deg] el_current_velocity : float Current el velocity [deg/s] + az_current_velocity : float + Current az velocity [deg/s] + bor_current_position : float + Current bor position [deg] + bor_commanded_position : float + Commanded bor position [deg] last_updated : float Time of last update [sec] """ @@ -186,11 +195,14 @@ class ACUState: max_el: float max_time_since_update: float mount_velocity_grip_thresh: float + grip_max_boresight_angle: float el_current_position: Optional[float] = None el_commanded_position: Optional[float] = None el_current_velocity: Optional[float] = None az_current_velocity: Optional[float] = None + bor_current_position: Optional[float] = None + bor_commanded_position: Optional[float] = None last_updated: Optional[float] = None @@ -207,6 +219,8 @@ def update(self): self.el_commanded_position = d['Elevation commanded position'] self.el_current_velocity = d['Elevation current velocity'] self.az_current_velocity = d['Azimuth current velocity'] + self.bor_current_position = d['Boresight current position'] + self.bor_commanded_position = d['Boresight commanded position'] t = d.get('timestamp_agent') if t is None: @@ -279,6 +293,7 @@ def from_args(cls, args: argparse.Namespace): max_el=args.acu_max_el, max_time_since_update=args.acu_max_time_since_update, mount_velocity_grip_thresh=args.mount_velocity_grip_thresh, + grip_max_boresight_angle=args.grip_max_boresight_angle, ) log.info("ACU state checking enabled: instance_id={id}", id=self.acu.instance_id) @@ -640,14 +655,12 @@ class GripHWP: """ Grips the HWP """ - pass @dataclass class UngripHWP: """ Ungrips the HWP """ - pass @dataclass class Abort: @@ -889,6 +902,16 @@ def check_ok_for_grip(): f"ACU el-velocity is {acu.el_current_velocity} deg/s, " f"above threshold of {acu.mount_velocity_grip_thresh} deg/s" ) + if np.abs(acu.bor_current_position) > np.abs(acu.grip_max_boresight_angle): + raise RuntimeError( + f"boresight current angle of {acu.bor_current_position} deg, " + f"above threshold of {acu.grip_max_boresight_angle} deg" + ) + if np.abs(acu.bor_commanded_position) > np.abs(acu.grip_max_boresight_angle): + raise RuntimeError( + f"boresight commanded angle of {acu.bor_commanded_position} deg, " + f"above threshold of {acu.grip_max_boresight_angle} deg" + ) if isinstance(state, ControlState.PIDToFreq): check_acu_ok_for_spinup() @@ -1010,12 +1033,12 @@ def check_ok_for_grip(): raise RuntimeError("Cannot grip HWP while spinning") check_ok_for_grip() self.run_and_validate(clients.gripper.grip) - self.action.set_state(ControlState.Done(success=state.success)) + self.action.set_state(ControlState.Done(success=True)) elif isinstance(state, ControlState.UngripHWP): check_ok_for_grip() self.run_and_validate(clients.gripper.ungrip) - self.action.set_state(ControlState.Done(success=state.success)) + self.action.set_state(ControlState.Done(success=True)) elif isinstance(state, ControlState.Brake): self.run_and_validate( @@ -1719,6 +1742,10 @@ def make_parser(parser=None): '--mount-vel-grip-thresh', type=float, default=0.005, help="Max mount velocity (both az and el) for gripping the HWP" ) + pgroup.add_argument( + '--grip-max-boresight-angle', type=float, default=1., + help="Maximum absolute value of boresight angle (deg) for gripping the HWP" + ) pgroup.add_argument('--forward-dir', choices=['cw', 'ccw'], default="cw", help="Whether the PID 'forward' direction is cw or ccw") From 304c249e76a5556bcfa93f84bc70cbcfd5bb7203 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 2 Aug 2024 23:24:29 +0000 Subject: [PATCH 03/14] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- socs/agents/hwp_supervisor/agent.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/socs/agents/hwp_supervisor/agent.py b/socs/agents/hwp_supervisor/agent.py index 2857f5a23..6e2212ef0 100644 --- a/socs/agents/hwp_supervisor/agent.py +++ b/socs/agents/hwp_supervisor/agent.py @@ -10,7 +10,7 @@ import ocs import txaio from ocs import client_http, ocs_agent, site_config -from ocs.client_http import ControlClientError, ControlClient +from ocs.client_http import ControlClient, ControlClientError from ocs.ocs_client import OCSClient, OCSReply from ocs.ocs_twisted import Pacemaker From 0dadc2c7d303acd11ecedf2bcdbb75d2e071c6fd Mon Sep 17 00:00:00 2001 From: jlashner Date: Fri, 2 Aug 2024 19:52:13 -0400 Subject: [PATCH 04/14] Expose supervisor control state info with rest of hwp state --- socs/agents/hwp_supervisor/agent.py | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/socs/agents/hwp_supervisor/agent.py b/socs/agents/hwp_supervisor/agent.py index 6e2212ef0..2259cf87e 100644 --- a/socs/agents/hwp_supervisor/agent.py +++ b/socs/agents/hwp_supervisor/agent.py @@ -4,7 +4,7 @@ import time import traceback from dataclasses import asdict, dataclass, field -from typing import Dict, List, Literal, Optional +from typing import Dict, List, Literal, Optional, Any import numpy as np import ocs @@ -263,6 +263,8 @@ class HWPState: acu: Optional[ACUState] = None + supervisor_control_state: Optional[Dict[str, Any]] = None + @classmethod def from_args(cls, args: argparse.Namespace): log = txaio.make_logger() # pylint: disable=E1101 @@ -1365,8 +1367,14 @@ def spin_control(self, session, params): """ clients = self._get_hwp_clients() + def update_supervisor_state(): + state_info = self.control_state_machine.action.cur_state_info.encode() + self.hwp_state.supervisor_control_state = state_info + while session.status in ['starting', 'running']: + update_supervisor_state() self.control_state_machine.update(clients, self.hwp_state) + update_supervisor_state() session.data = { 'current_action': self.control_state_machine.action.encode(), 'action_history': [a.encode() for a in self.control_state_machine.action_history], @@ -1538,7 +1546,7 @@ def pmx_off(self, session, params): return action.success, f"Completed with state: {action.cur_state_info.state}" def grip_hwp(self, session, params): - """grip() + """grip_hwp() **Task** - Grip the HWP if ACU and HWP state passes checks. @@ -1562,7 +1570,7 @@ def grip_hwp(self, session, params): return action.success, f"Completed with state: {action.cur_state_info.state}" def ungrip_hwp(self, session, params): - """ungrip() + """ungrip_hwp() **Task** - Ungrip the HWP if ACU and HWP state passes checks. From 84b1a2a5c56701cb68711d815c4a70db602a0acb Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 2 Aug 2024 23:52:42 +0000 Subject: [PATCH 05/14] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- socs/agents/hwp_supervisor/agent.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/socs/agents/hwp_supervisor/agent.py b/socs/agents/hwp_supervisor/agent.py index 2259cf87e..50fa2f248 100644 --- a/socs/agents/hwp_supervisor/agent.py +++ b/socs/agents/hwp_supervisor/agent.py @@ -4,7 +4,7 @@ import time import traceback from dataclasses import asdict, dataclass, field -from typing import Dict, List, Literal, Optional, Any +from typing import Any, Dict, List, Literal, Optional import numpy as np import ocs From 9b86708ff9582606e9b54781bf793fb479c504e8 Mon Sep 17 00:00:00 2001 From: jlashner Date: Fri, 2 Aug 2024 20:30:19 -0400 Subject: [PATCH 06/14] Add current_state_type to spin control function --- socs/agents/hwp_supervisor/agent.py | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/socs/agents/hwp_supervisor/agent.py b/socs/agents/hwp_supervisor/agent.py index 50fa2f248..39d874931 100644 --- a/socs/agents/hwp_supervisor/agent.py +++ b/socs/agents/hwp_supervisor/agent.py @@ -1370,16 +1370,21 @@ def spin_control(self, session, params): def update_supervisor_state(): state_info = self.control_state_machine.action.cur_state_info.encode() self.hwp_state.supervisor_control_state = state_info + if session.data: + session.data['current_state_type'] = self.control_state_machine.action.cur_state_info.state_type while session.status in ['starting', 'running']: + # Update session data beginning and end of update call, because its + # possible for an action change to happen due to external request from + # task. update_supervisor_state() self.control_state_machine.update(clients, self.hwp_state) update_supervisor_state() - session.data = { + session.data.update({ 'current_action': self.control_state_machine.action.encode(), 'action_history': [a.encode() for a in self.control_state_machine.action_history], - 'timestamp': time.time() - } + 'timestamp': time.time(), + }) if params['test_mode']: break time.sleep(1) From 9dba2dbfc3a9af615356c581143560f053d82afb Mon Sep 17 00:00:00 2001 From: Jack Lashner Date: Mon, 12 Aug 2024 19:08:28 -0400 Subject: [PATCH 07/14] Adds ACU blocking flag and handshake to HWP Supervisor --- socs/agents/hwp_supervisor/agent.py | 168 ++++++++++++++++++---------- 1 file changed, 107 insertions(+), 61 deletions(-) diff --git a/socs/agents/hwp_supervisor/agent.py b/socs/agents/hwp_supervisor/agent.py index 39d874931..0e2ffcd54 100644 --- a/socs/agents/hwp_supervisor/agent.py +++ b/socs/agents/hwp_supervisor/agent.py @@ -5,6 +5,7 @@ import traceback from dataclasses import asdict, dataclass, field from typing import Any, Dict, List, Literal, Optional +from contextlib import contextmanager import numpy as np import ocs @@ -188,7 +189,8 @@ class ACUState: bor_commanded_position : float Commanded bor position [deg] last_updated : float - Time of last update [sec] + Time of last update [sec]. If this is None, you cannot trust other + state variables. """ instance_id: str min_el: float @@ -197,14 +199,18 @@ class ACUState: mount_velocity_grip_thresh: float grip_max_boresight_angle: float - el_current_position: Optional[float] = None - el_commanded_position: Optional[float] = None - el_current_velocity: Optional[float] = None - az_current_velocity: Optional[float] = None - bor_current_position: Optional[float] = None - bor_commanded_position: Optional[float] = None - last_updated: Optional[float] = None + el_current_position: float = 0.0 + el_commanded_position: float = 0.0 + el_current_velocity: float = 0.0 + az_current_velocity: float = 0.0 + bor_current_position: float = 0.0 + bor_commanded_position: float = 0.0 + + # This will only be set by spin-control agent for communication with ACU + request_block_ACU_motion: bool = False + ACU_motion_blocked: Optional[bool] = None # This flag is only set by the ACU agent + use_acu_blocking: bool = False def update(self): op = get_op_data(self.instance_id, 'monitor') @@ -296,6 +302,7 @@ def from_args(cls, args: argparse.Namespace): max_time_since_update=args.acu_max_time_since_update, mount_velocity_grip_thresh=args.mount_velocity_grip_thresh, grip_max_boresight_angle=args.grip_max_boresight_angle, + use_acu_blocking=args.use_acu_blocking, ) log.info("ACU state checking enabled: instance_id={id}", id=self.acu.instance_id) @@ -788,6 +795,89 @@ def sleep_until_complete(self, session=None, dt=1): time.sleep(dt) +@contextmanager +def ensure_grip_safety(hwp_state: HWPState, timeout: float=60.): + """ + Run required checks for gripper safety. This will check ACU parameters such + as az/el position and velocity. If `use_acu_blocking` is set, this will + also attempt to block out ACU motion for the duration of the operation. + + Args + ------ + hwp_state : HWPState + HWP state object. In addition to reading ACU state vars, this will + set the `request_block_ACU_motion` flag if `use_acu_blocking` is set. + timeout: float + Timeout for waiting for the ACU blockout before an error will be raised. + """ + if hwp_state.pid_current_freq is None: + raise RuntimeError("Cannot determine current HWP Freq") + + if np.abs(hwp_state.pid_current_freq) > 0.02: + raise RuntimeError("Cannot grip HWP while spinning") + + acu = hwp_state.acu + if acu is None: + yield + return + + # Run checks of ACU state + if acu.last_updated is None: + raise RuntimeError( + f"No ACU data has been received from instance-id {acu.instance_id}" + ) + + tdiff = time.time() - acu.last_updated + if tdiff > acu.max_time_since_update: + raise RuntimeError(f"ACU state has not been updated in {tdiff} sec") + + if not (acu.min_el <= acu.el_current_position <= acu.max_el): + raise RuntimeError( + f"ACU elevation is {acu.el_current_position} deg, " + f"outside of allowed range ({acu.min_el}, {acu.max_el})" + ) + if not (acu.min_el <= acu.el_commanded_position <= acu.max_el): + raise RuntimeError( + f"ACU commanded elevation is {acu.el_commanded_position} deg, " + f"outside of allowed range ({acu.min_el}, {acu.max_el})" + ) + if np.abs(acu.az_current_velocity) > np.abs(acu.mount_velocity_grip_thresh): + raise RuntimeError( + f"ACU az-velocity is {acu.az_current_velocity} deg/s, " + f"above threshold of {acu.mount_velocity_grip_thresh} deg/s" + ) + if np.abs(acu.el_current_velocity) > np.abs(acu.mount_velocity_grip_thresh): + raise RuntimeError( + f"ACU el-velocity is {acu.el_current_velocity} deg/s, " + f"above threshold of {acu.mount_velocity_grip_thresh} deg/s" + ) + if np.abs(acu.bor_current_position) > np.abs(acu.grip_max_boresight_angle): + raise RuntimeError( + f"boresight current angle of {acu.bor_current_position} deg, " + f"above threshold of {acu.grip_max_boresight_angle} deg" + ) + if np.abs(acu.bor_commanded_position) > np.abs(acu.grip_max_boresight_angle): + raise RuntimeError( + f"boresight commanded angle of {acu.bor_commanded_position} deg, " + f"above threshold of {acu.grip_max_boresight_angle} deg" + ) + + if not acu.use_acu_blocking: + yield + return + + try: #If use_acu_blocking, do handshake with ACU agent to block motino + acu.request_block_ACU_motion = True + start_time = time.time() + while not acu.ACU_motion_blocked: + if time.time() - start_time > timeout: + raise RuntimeError("ACU motion was not blocked within timeout") + time.sleep(1) + yield + finally: + acu.request_block_ACU_motion = False + + class ControlStateMachine: def __init__(self): self.action: ControlAction = ControlAction(ControlState.Idle()) @@ -868,53 +958,6 @@ 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_ok_for_grip(): - """Validates telescope is in a state that is ok for gripping/ungripping""" - acu = hwp_state.acu - - if np.abs(hwp_state.pid_current_freq) > 0.02: - raise RuntimeError("Cannot grip HWP while spinning") - - if acu is not None: - if acu.last_updated is None: - raise RuntimeError( - f"No ACU data has been received from instance-id {acu.instance_id}" - ) - tdiff = time.time() - acu.last_updated - if tdiff > acu.max_time_since_update: - raise RuntimeError(f"ACU state has not been updated in {tdiff} sec") - - if not (acu.min_el <= acu.el_current_position <= acu.max_el): - raise RuntimeError( - f"ACU elevation is {acu.el_current_pos} deg, " - f"outside of allowed range ({acu.min_el}, {acu.max_el})" - ) - if not (acu.min_el <= acu.el_commanded_position <= acu.max_el): - raise RuntimeError( - f"ACU commanded elevation is {acu.el_commanded_position} deg, " - f"outside of allowed range ({acu.min_el}, {acu.max_el})" - ) - if np.abs(acu.az_current_velocity) > np.abs(acu.mount_velocity_grip_thresh): - raise RuntimeError( - f"ACU az-velocity is {acu.az_current_velocity} deg/s, " - f"above threshold of {acu.mount_velocity_grip_thresh} deg/s" - ) - if np.abs(acu.el_current_velocity) > np.abs(acu.mount_velocity_grip_thresh): - raise RuntimeError( - f"ACU el-velocity is {acu.el_current_velocity} deg/s, " - f"above threshold of {acu.mount_velocity_grip_thresh} deg/s" - ) - if np.abs(acu.bor_current_position) > np.abs(acu.grip_max_boresight_angle): - raise RuntimeError( - f"boresight current angle of {acu.bor_current_position} deg, " - f"above threshold of {acu.grip_max_boresight_angle} deg" - ) - if np.abs(acu.bor_commanded_position) > np.abs(acu.grip_max_boresight_angle): - raise RuntimeError( - f"boresight commanded angle of {acu.bor_commanded_position} deg, " - f"above threshold of {acu.grip_max_boresight_angle} deg" - ) - if isinstance(state, ControlState.PIDToFreq): check_acu_ok_for_spinup() self.run_and_validate(clients.pid.set_direction, @@ -1031,15 +1074,13 @@ def check_ok_for_grip(): self.action.set_state(ControlState.Done(success=state.success)) elif isinstance(state, ControlState.GripHWP): - if np.abs(hwp_state.pid_current_freq) > 0.02: - raise RuntimeError("Cannot grip HWP while spinning") - check_ok_for_grip() - self.run_and_validate(clients.gripper.grip) + with ensure_grip_safety(hwp_state): + self.run_and_validate(clients.gripper.grip) self.action.set_state(ControlState.Done(success=True)) elif isinstance(state, ControlState.UngripHWP): - check_ok_for_grip() - self.run_and_validate(clients.gripper.ungrip) + with ensure_grip_safety(hwp_state): + self.run_and_validate(clients.gripper.ungrip) self.action.set_state(ControlState.Done(success=True)) elif isinstance(state, ControlState.Brake): @@ -1759,6 +1800,11 @@ def make_parser(parser=None): '--grip-max-boresight-angle', type=float, default=1., help="Maximum absolute value of boresight angle (deg) for gripping the HWP" ) + pgroup.add_argument( + '--use-acu-blocking', action='store_true', + help="If True, will use blocking flags to make sure ACU is not moving " + "during HWP gripper commands." + ) pgroup.add_argument('--forward-dir', choices=['cw', 'ccw'], default="cw", help="Whether the PID 'forward' direction is cw or ccw") From 9bd11926ff5f72201e36bcd08cf87fbdc007c604 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 12 Aug 2024 23:08:41 +0000 Subject: [PATCH 08/14] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- socs/agents/hwp_supervisor/agent.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/socs/agents/hwp_supervisor/agent.py b/socs/agents/hwp_supervisor/agent.py index 0e2ffcd54..833859fd4 100644 --- a/socs/agents/hwp_supervisor/agent.py +++ b/socs/agents/hwp_supervisor/agent.py @@ -3,9 +3,9 @@ import threading import time import traceback +from contextlib import contextmanager from dataclasses import asdict, dataclass, field from typing import Any, Dict, List, Literal, Optional -from contextlib import contextmanager import numpy as np import ocs @@ -209,7 +209,7 @@ class ACUState: # This will only be set by spin-control agent for communication with ACU request_block_ACU_motion: bool = False - ACU_motion_blocked: Optional[bool] = None # This flag is only set by the ACU agent + ACU_motion_blocked: Optional[bool] = None # This flag is only set by the ACU agent use_acu_blocking: bool = False def update(self): @@ -796,7 +796,7 @@ def sleep_until_complete(self, session=None, dt=1): @contextmanager -def ensure_grip_safety(hwp_state: HWPState, timeout: float=60.): +def ensure_grip_safety(hwp_state: HWPState, timeout: float = 60.): """ Run required checks for gripper safety. This will check ACU parameters such as az/el position and velocity. If `use_acu_blocking` is set, this will @@ -866,7 +866,7 @@ def ensure_grip_safety(hwp_state: HWPState, timeout: float=60.): yield return - try: #If use_acu_blocking, do handshake with ACU agent to block motino + try: # If use_acu_blocking, do handshake with ACU agent to block motino acu.request_block_ACU_motion = True start_time = time.time() while not acu.ACU_motion_blocked: From 811858ad075671b2306c16e9a3edcfff579c8797 Mon Sep 17 00:00:00 2001 From: ykyohei <38639108+ykyohei@users.noreply.github.com> Date: Thu, 15 Aug 2024 18:19:10 +0000 Subject: [PATCH 09/14] fix typo and register gripper tasks --- socs/agents/hwp_supervisor/agent.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/socs/agents/hwp_supervisor/agent.py b/socs/agents/hwp_supervisor/agent.py index 833859fd4..518df8c89 100644 --- a/socs/agents/hwp_supervisor/agent.py +++ b/socs/agents/hwp_supervisor/agent.py @@ -1793,7 +1793,7 @@ def make_parser(parser=None): help="Max amount of time since last ACU update before allowing HWP spin up", ) pgroup.add_argument( - '--mount-vel-grip-thresh', type=float, default=0.005, + '--mount-velocity-grip-thresh', type=float, default=0.005, help="Max mount velocity (both az and el) for gripping the HWP" ) pgroup.add_argument( @@ -1828,9 +1828,11 @@ def main(args=None): agent.register_task('set_const_voltage', hwp.set_const_voltage) agent.register_task('brake', hwp.brake) agent.register_task('pmx_off', hwp.pmx_off) - agent.register_task('abort_action', hwp.abort_action) + agent.register_task('grip_hwp', hwp.grip_hwp) + agent.register_task('ungrip_hwp', hwp.ungrip_hwp) agent.register_task('enable_driver_board', hwp.enable_driver_board) agent.register_task('disable_driver_board', hwp.disable_driver_board) + agent.register_task('abort_action', hwp.abort_action) runner.run(agent, auto_reconnect=True) From 053719137b68a6024aaa7436a673ff846cb019b8 Mon Sep 17 00:00:00 2001 From: Jack Lashner Date: Thu, 5 Sep 2024 19:08:05 -0400 Subject: [PATCH 10/14] Adds ACU blocking flag timestamp and timeout --- socs/agents/hwp_supervisor/agent.py | 25 ++++++++++++++++++------- 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/socs/agents/hwp_supervisor/agent.py b/socs/agents/hwp_supervisor/agent.py index 518df8c89..beaa4715b 100644 --- a/socs/agents/hwp_supervisor/agent.py +++ b/socs/agents/hwp_supervisor/agent.py @@ -5,7 +5,7 @@ import traceback from contextlib import contextmanager from dataclasses import asdict, dataclass, field -from typing import Any, Dict, List, Literal, Optional +from typing import Any, Dict, List, Literal, Optional, Generator import numpy as np import ocs @@ -208,9 +208,16 @@ class ACUState: bor_commanded_position: float = 0.0 # This will only be set by spin-control agent for communication with ACU - request_block_ACU_motion: bool = False + request_block_motion: bool = False + request_block_motion_timestamp: float = 0.0 + ACU_motion_blocked: Optional[bool] = None # This flag is only set by the ACU agent use_acu_blocking: bool = False + block_motion_timeout: float= 60.0 + + def set_request_block_motion(self, state: bool) -> None: + self.request_block_motion = state + self.request_block_motion_timestamp = time.time() def update(self): op = get_op_data(self.instance_id, 'monitor') @@ -303,6 +310,7 @@ def from_args(cls, args: argparse.Namespace): mount_velocity_grip_thresh=args.mount_velocity_grip_thresh, grip_max_boresight_angle=args.grip_max_boresight_angle, use_acu_blocking=args.use_acu_blocking, + block_motion_timeout=args.acu_block_motion_timeout, ) log.info("ACU state checking enabled: instance_id={id}", id=self.acu.instance_id) @@ -796,7 +804,7 @@ def sleep_until_complete(self, session=None, dt=1): @contextmanager -def ensure_grip_safety(hwp_state: HWPState, timeout: float = 60.): +def ensure_grip_safety(hwp_state: HWPState) -> Generator[None, None, None]: """ Run required checks for gripper safety. This will check ACU parameters such as az/el position and velocity. If `use_acu_blocking` is set, this will @@ -867,15 +875,14 @@ def ensure_grip_safety(hwp_state: HWPState, timeout: float = 60.): return try: # If use_acu_blocking, do handshake with ACU agent to block motino - acu.request_block_ACU_motion = True - start_time = time.time() + acu.set_request_block_motion(True) while not acu.ACU_motion_blocked: - if time.time() - start_time > timeout: + if time.time() - acu.request_block_motion_timestamp > acu.block_motion_timeout: raise RuntimeError("ACU motion was not blocked within timeout") time.sleep(1) yield finally: - acu.request_block_ACU_motion = False + acu.set_request_block_motion(False) class ControlStateMachine: @@ -1805,6 +1812,10 @@ def make_parser(parser=None): help="If True, will use blocking flags to make sure ACU is not moving " "during HWP gripper commands." ) + pgroup.add_argument( + '--acu-block-motion-timeout', type=float, default=60., + help="Time to wait for ACU motion to be blocked before aborting gripper command." + ) pgroup.add_argument('--forward-dir', choices=['cw', 'ccw'], default="cw", help="Whether the PID 'forward' direction is cw or ccw") From c395d358ba5dba316b4ba9fb836aff7971889817 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 5 Sep 2024 23:08:21 +0000 Subject: [PATCH 11/14] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- socs/agents/hwp_supervisor/agent.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/socs/agents/hwp_supervisor/agent.py b/socs/agents/hwp_supervisor/agent.py index beaa4715b..3246c12d7 100644 --- a/socs/agents/hwp_supervisor/agent.py +++ b/socs/agents/hwp_supervisor/agent.py @@ -5,7 +5,7 @@ import traceback from contextlib import contextmanager from dataclasses import asdict, dataclass, field -from typing import Any, Dict, List, Literal, Optional, Generator +from typing import Any, Dict, Generator, List, Literal, Optional import numpy as np import ocs @@ -213,7 +213,7 @@ class ACUState: ACU_motion_blocked: Optional[bool] = None # This flag is only set by the ACU agent use_acu_blocking: bool = False - block_motion_timeout: float= 60.0 + block_motion_timeout: float = 60.0 def set_request_block_motion(self, state: bool) -> None: self.request_block_motion = state From a173cd3659e1238f26095d580f27fd595aa21583 Mon Sep 17 00:00:00 2001 From: Jack Lashner Date: Fri, 6 Sep 2024 13:12:52 -0400 Subject: [PATCH 12/14] Adds acu instance id default and `--no-acu` option --- socs/agents/hwp_supervisor/agent.py | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/socs/agents/hwp_supervisor/agent.py b/socs/agents/hwp_supervisor/agent.py index beaa4715b..7dfdbc936 100644 --- a/socs/agents/hwp_supervisor/agent.py +++ b/socs/agents/hwp_supervisor/agent.py @@ -301,7 +301,7 @@ def from_args(cls, args: argparse.Namespace): else: log.warn("Driver Ibootbar id not set") - if args.acu_instance_id is not None: + if not args.no_acu: self.acu = ACUState( instance_id=args.acu_instance_id, min_el=args.acu_min_el, @@ -315,7 +315,7 @@ def from_args(cls, args: argparse.Namespace): log.info("ACU state checking enabled: instance_id={id}", id=self.acu.instance_id) else: - log.info("ACU state checking disabled.") + log.warn("The no-acu option has been set. ACU state checking disabled.") return self @@ -886,7 +886,7 @@ def ensure_grip_safety(hwp_state: HWPState) -> Generator[None, None, None]: class ControlStateMachine: - def __init__(self): + def __init__(self) -> None: self.action: ControlAction = ControlAction(ControlState.Idle()) self.action_history: List[ControlAction] = [] self.max_action_history_count = 100 @@ -959,7 +959,7 @@ def check_acu_ok_for_spinup(): if tdiff > acu.max_time_since_update: raise RuntimeError(f"ACU state has not been updated in {tdiff} sec") if not (acu.min_el <= acu.el_current_position <= acu.max_el): - raise RuntimeError(f"ACU elevation is {acu.el_current_pos} deg, " + raise RuntimeError(f"ACU elevation is {acu.el_current_position} deg, " f"outside of allowed range ({acu.min_el}, {acu.max_el})") if not (acu.min_el <= acu.el_commanded_position <= acu.max_el): raise RuntimeError(f"ACU commanded elevation is {acu.el_commanded_position} deg, " @@ -1783,9 +1783,13 @@ def make_parser(parser=None): help="Type of agent used for controlling the gripper power") pgroup.add_argument( - '--acu-instance-id', - help="Instance ID for the ACU agent. This is required for checks of ACU " - "postiion and velocity before HWP commands." + '--acu-instance-id', default='acu', + help="Instance ID for the ACU agent." + ) + pgroup.add_argument( + '--no-acu', action='store_true', + help="If set, will not attempt to connect to the ACU or perform any ACU " + "checks for grip and spin-up" ) pgroup.add_argument( '--acu-min-el', type=float, default=48.0, From f0266cbe5eaf3bda9f08dbbf36ad9f3a2299f838 Mon Sep 17 00:00:00 2001 From: Jack Lashner Date: Fri, 6 Sep 2024 13:30:58 -0400 Subject: [PATCH 13/14] Adds checks on pid last update time for gripper safety --- socs/agents/hwp_supervisor/agent.py | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/socs/agents/hwp_supervisor/agent.py b/socs/agents/hwp_supervisor/agent.py index 2ed617bf7..9190e098f 100644 --- a/socs/agents/hwp_supervisor/agent.py +++ b/socs/agents/hwp_supervisor/agent.py @@ -261,6 +261,7 @@ class HWPState: pid_target_freq: Optional[float] = None pid_direction: Optional[str] = None pid_last_updated: Optional[float] = None + pid_max_time_since_update: float = 60.0 pmx_current: Optional[float] = None pmx_voltage: Optional[float] = None @@ -285,6 +286,7 @@ def from_args(cls, args: argparse.Namespace): temp_field=args.ybco_temp_field, temp_thresh=args.ybco_temp_thresh, ups_minutes_remaining_thresh=args.ups_minutes_remaining_thresh, + pid_max_time_since_update=args.pid_max_time_since_update, ) if args.gripper_iboot_id is not None: @@ -818,9 +820,14 @@ def ensure_grip_safety(hwp_state: HWPState) -> Generator[None, None, None]: timeout: float Timeout for waiting for the ACU blockout before an error will be raised. """ - if hwp_state.pid_current_freq is None: + now = time.time() + if hwp_state.pid_current_freq is None or hwp_state.pid_last_updated is None: raise RuntimeError("Cannot determine current HWP Freq") + tdiff = now - hwp_state.pid_last_updated + if tdiff > hwp_state.pid_max_time_since_update: + raise RuntimeError(f"HWP PID state has not been updated in {tdiff} sec") + if np.abs(hwp_state.pid_current_freq) > 0.02: raise RuntimeError("Cannot grip HWP while spinning") @@ -1755,6 +1762,10 @@ def make_parser(parser=None): pgroup.add_argument('--ups-minutes-remaining-thresh', type=float, help="Threshold for UPS minutes remaining before a " "shutdown is triggered") + pgroup.add_argument( + '--pid-max-time-since-update', type=float, default=60.0, + help="Max amount of time since last PID update before data is considered stale.", + ) pgroup.add_argument( '--driver-iboot-id', From b6a6f756a213f1669b34aad4871a8e0284f88c90 Mon Sep 17 00:00:00 2001 From: Jack Lashner Date: Fri, 6 Sep 2024 13:35:36 -0400 Subject: [PATCH 14/14] Adds better logging to ensure_grip_safety --- socs/agents/hwp_supervisor/agent.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/socs/agents/hwp_supervisor/agent.py b/socs/agents/hwp_supervisor/agent.py index 9190e098f..1aaffc0ef 100644 --- a/socs/agents/hwp_supervisor/agent.py +++ b/socs/agents/hwp_supervisor/agent.py @@ -806,7 +806,7 @@ def sleep_until_complete(self, session=None, dt=1): @contextmanager -def ensure_grip_safety(hwp_state: HWPState) -> Generator[None, None, None]: +def ensure_grip_safety(hwp_state: HWPState, log: txaio.ILogger) -> Generator[None, None, None]: """ Run required checks for gripper safety. This will check ACU parameters such as az/el position and velocity. If `use_acu_blocking` is set, this will @@ -821,6 +821,7 @@ def ensure_grip_safety(hwp_state: HWPState) -> Generator[None, None, None]: Timeout for waiting for the ACU blockout before an error will be raised. """ now = time.time() + if hwp_state.pid_current_freq is None or hwp_state.pid_last_updated is None: raise RuntimeError("Cannot determine current HWP Freq") @@ -830,6 +831,7 @@ def ensure_grip_safety(hwp_state: HWPState) -> Generator[None, None, None]: if np.abs(hwp_state.pid_current_freq) > 0.02: raise RuntimeError("Cannot grip HWP while spinning") + log.info("Rotation safety checks have passed") acu = hwp_state.acu if acu is None: @@ -876,6 +878,7 @@ def ensure_grip_safety(hwp_state: HWPState) -> Generator[None, None, None]: f"boresight commanded angle of {acu.bor_commanded_position} deg, " f"above threshold of {acu.grip_max_boresight_angle} deg" ) + log.info("ACU safety checks have passed") if not acu.use_acu_blocking: yield @@ -883,12 +886,15 @@ def ensure_grip_safety(hwp_state: HWPState) -> Generator[None, None, None]: try: # If use_acu_blocking, do handshake with ACU agent to block motino acu.set_request_block_motion(True) + log.info("Requesting ACU to block motion") while not acu.ACU_motion_blocked: if time.time() - acu.request_block_motion_timestamp > acu.block_motion_timeout: raise RuntimeError("ACU motion was not blocked within timeout") time.sleep(1) + log.info("ACU motion has been blocked") yield finally: + log.info("Releasing ACU motion block") acu.set_request_block_motion(False) @@ -1088,12 +1094,12 @@ def check_acu_ok_for_spinup(): self.action.set_state(ControlState.Done(success=state.success)) elif isinstance(state, ControlState.GripHWP): - with ensure_grip_safety(hwp_state): + with ensure_grip_safety(hwp_state, self.log): self.run_and_validate(clients.gripper.grip) self.action.set_state(ControlState.Done(success=True)) elif isinstance(state, ControlState.UngripHWP): - with ensure_grip_safety(hwp_state): + with ensure_grip_safety(hwp_state, self.log): self.run_and_validate(clients.gripper.ungrip) self.action.set_state(ControlState.Done(success=True))