From ace361c62fd9be3a8201c9ccec3043b7199e3ce2 Mon Sep 17 00:00:00 2001 From: Te-Wei Tsai Date: Fri, 17 Jan 2025 13:43:17 -0300 Subject: [PATCH 1/8] Improve the warmup_hexapod.py. --- doc/news/DM-48447.perf.rst | 1 + .../externalscripts/maintel/warmup_hexapod.py | 195 +++++++++++++++--- 2 files changed, 169 insertions(+), 27 deletions(-) create mode 100644 doc/news/DM-48447.perf.rst diff --git a/doc/news/DM-48447.perf.rst b/doc/news/DM-48447.perf.rst new file mode 100644 index 000000000..08f1561b2 --- /dev/null +++ b/doc/news/DM-48447.perf.rst @@ -0,0 +1 @@ +Improve the warmup_hexapod.py to recover from the failure and change the step size in the runtime. diff --git a/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py b/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py index f1e258979..a324e1c4e 100644 --- a/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py +++ b/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py @@ -23,10 +23,9 @@ import asyncio from collections.abc import Iterable -import numpy as np import yaml from lsst.ts import salobj -from lsst.ts.idl.enums.MTHexapod import SalIndex +from lsst.ts.idl.enums.MTHexapod import EnabledSubstate, SalIndex from lsst.ts.observatory.control.maintel.mtcs import MTCS, MTCSUsages @@ -213,31 +212,133 @@ async def warm_up(self): for step, sleep_time in zip(self.config.step_size, self.config.sleep_time): await self.single_loop(step, sleep_time) - async def move_stepwise(self, start, stop, step, sleep_time): - """Moves the hexapod from `start` to `stop` in `steps`. + async def move_stepwise( + self, start: float, stop: float, step_initial: float, sleep_time: float + ): + """Moves the hexapod from `start` to `stop` that begins from + `step_initial`. The step size is updated based on the result of the + movement. Parameters ---------- - start : float + start : `float` Initial position. - stop : float + stop : `float` Final position. - step : float - Step size. - sleep_time : float + step_initial : `float` + Initial step size. + sleep_time : `float` Time between movements. """ - axes = "xyzuvw" - for new_pos in np.arange(start, stop, step): - pos = await self.hexapod.tel_application.aget() + position_current = start + position_next = start + step_current = step_initial + while position_current != stop: + # Make sure the next position is not beyond the stop + position_next = position_current + step_current + if step_initial >= 0: + if position_next >= stop: + position_next = stop + else: + if position_next <= stop: + position_next = stop + + # Do the movement + all_positions = await self._get_position() + all_positions[self.config.axis] = position_next + + is_successful = await self._move_hexapod(**all_positions) + + # Update the step size based on the result + step_current_update = ( + (step_current + step_initial) if is_successful else (step_current / 2) + ) + step_current = ( + step_current_update + if (abs(step_current_update) >= abs(step_initial)) + else step_initial + ) - all_positions = {axes[i]: pos.position[i] for i in range(len(axes))} - all_positions[self.config.axis] = new_pos + # Update the current position + if is_successful: + position_current = position_next + else: + position_fail = await self._get_position() + position_current = position_fail[self.config.axis] - await self.move_hexapod(**all_positions) await asyncio.sleep(sleep_time) + async def _get_position(self) -> dict: + """Get the current position of the hexapod. + + Returns + ------- + `dict` + Current position of the hexapod. + """ + + pos = await self.hexapod.tel_application.aget() + + axes = "xyzuvw" + return {axes[i]: pos.position[i] for i in range(len(axes))} + + async def _move_hexapod( + self, + x: float, + y: float, + z: float, + u: float, + v: float, + w: float = 0.0, + ) -> bool: + """Move the hexapod to the given position. + + Parameters + ---------- + x : `float` + Hexapod-x position (microns). + y : `float` + Hexapod-y position (microns). + z : `float` + Hexapod-z position (microns). + u : `float` + Hexapod-u angle (degrees). + v : `float` + Hexapod-v angle (degrees). + w : `float`, optional + Hexapod-w angle (degrees). Default 0. + + Returns + ------- + `bool` + `True` if the movement is successful, `False` otherwise. + """ + + try: + await self.move_hexapod(x, y, z, u, v, w=w) + + return True + except Exception: + # If the hexapod is moving, stop it + controller_enabled_state = ( + self.hexapod.evt_controllerState.get().enabledSubstate + ) + if controller_enabled_state == EnabledSubstate.MOVING_POINT_TO_POINT: + self.log.info("Stop the hexapod CSC.") + await self.hexapod.cmd_stop.set_start() + + # If the hexapod is in fault, recover it + state = self.hexapod.evt_summaryState.get() + if state == salobj.State.FAULT: + self.log.info("Recover the hexapod CSC from the Fault.") + await salobj.set_summary_state(self.hexapod, salobj.State.ENABLED) + + # Wait for a few seconds + await asyncio.sleep(5.0) + + return False + async def single_loop(self, step, sleep_time): """ Do a full loop moving from the current position to the positive limit @@ -252,26 +353,66 @@ async def single_loop(self, step, sleep_time): Time between movements. """ self.log.info(f"Start loop with {step} step and {sleep_time} sleep time.") - idx = "xyzuvw".index(self.config.axis) - pos = await self.hexapod.tel_application.aget() - current_pos = pos.position[idx] # current position - self.log.debug( - f"Move from current position to maximum position in steps of {step}" - ) + # Move to the origin first + await self._move_to_origin() + + # Positive direction + self.log.debug("Move from 0 to maximum position") await self.move_stepwise( - current_pos, + 0.0, self.config.max_position, step, sleep_time, ) - self.log.debug( - f"Move from maximum position to minimum position in steps of {step}" + self.log.debug("Move from maximum position back to 0") + await self.move_stepwise( + self.config.max_position, + 0.0, + -step, + sleep_time, ) + + # Negative direction + self.log.debug("Move from 0 to minimum position") await self.move_stepwise( - self.config.max_position, -self.config.max_position, -step, sleep_time + 0.0, + -self.config.max_position, + -step, + sleep_time, ) - self.log.debug(f"Move from minimum position back to 0 in steps of {step}") - await self.move_stepwise(-self.config.max_position, 1, step, sleep_time) + self.log.debug("Move from minimum position back to 0") + await self.move_stepwise( + -self.config.max_position, + 0.0, + step, + sleep_time, + ) + + async def _move_to_origin(self, max_count: int = 5) -> None: + """Move to the origin. + + Parameters + ---------- + max_count : `int`, optional + Maximum count to try. (the default is 5) + + Raises + ------ + `RuntimeError` + When the hexapod fails to move to the origin. + """ + + count = 0 + while count < max_count: + is_done = await self._move_hexapod(0.0, 0.0, 0.0, 0.0, 0.0, w=0.0) + if is_done: + return + else: + count += 1 + + raise RuntimeError( + f"Failed to move the hexapod to the origin. with {max_count} tries" + ) From 2ccc0dbc4e8656e0d2f33e3b916820679571ba32 Mon Sep 17 00:00:00 2001 From: Te-Wei Tsai Date: Wed, 22 Jan 2025 08:50:50 -0300 Subject: [PATCH 2/8] Improve the log messages and the error handling.. --- .../externalscripts/maintel/warmup_hexapod.py | 57 +++++++++++++++---- 1 file changed, 47 insertions(+), 10 deletions(-) diff --git a/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py b/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py index a324e1c4e..58a39acea 100644 --- a/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py +++ b/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py @@ -213,7 +213,12 @@ async def warm_up(self): await self.single_loop(step, sleep_time) async def move_stepwise( - self, start: float, stop: float, step_initial: float, sleep_time: float + self, + start: float, + stop: float, + step_initial: float, + sleep_time: float, + max_count: int = 5, ): """Moves the hexapod from `start` to `stop` that begins from `step_initial`. The step size is updated based on the result of the @@ -229,8 +234,16 @@ async def move_stepwise( Initial step size. sleep_time : `float` Time between movements. + max_count : `int`, optional + Maximum count to try. (the default is 5) + + Raises + ------ + `RuntimeError` + When the hexapod fails to move in a single stage. """ + count = 0 position_current = start position_next = start step_current = step_initial @@ -244,6 +257,11 @@ async def move_stepwise( if position_next <= stop: position_next = stop + self.log.debug( + f"{self.hexapod_name} moves from {position_current} to " + f"{position_next} with step size: {step_current}" + ) + # Do the movement all_positions = await self._get_position() all_positions[self.config.axis] = position_next @@ -267,6 +285,18 @@ async def move_stepwise( position_fail = await self._get_position() position_current = position_fail[self.config.axis] + count += 1 + + if count >= max_count: + raise RuntimeError( + f"{self.hexapod_name} failed to move with {max_count} " + "tries in a single stage" + ) + + self.log.debug( + f"Current position of {self.hexapod_name} is {position_current}" + ) + await asyncio.sleep(sleep_time) async def _get_position(self) -> dict: @@ -319,19 +349,23 @@ async def _move_hexapod( await self.move_hexapod(x, y, z, u, v, w=w) return True - except Exception: + except (asyncio.CancelledError, TimeoutError): + self.log.exception( + f"Error moving the {self.hexapod_name} to {x=}, {y=}, {z=}, {u=}, {v=}." + ) + # If the hexapod is moving, stop it controller_enabled_state = ( self.hexapod.evt_controllerState.get().enabledSubstate ) if controller_enabled_state == EnabledSubstate.MOVING_POINT_TO_POINT: - self.log.info("Stop the hexapod CSC.") + self.log.info(f"Stop the {self.hexapod_name} CSC.") await self.hexapod.cmd_stop.set_start() # If the hexapod is in fault, recover it state = self.hexapod.evt_summaryState.get() if state == salobj.State.FAULT: - self.log.info("Recover the hexapod CSC from the Fault.") + self.log.info(f"Recover the {self.hexapod_name} CSC from the Fault.") await salobj.set_summary_state(self.hexapod, salobj.State.ENABLED) # Wait for a few seconds @@ -352,13 +386,16 @@ async def single_loop(self, step, sleep_time): sleep_time : float Time between movements. """ - self.log.info(f"Start loop with {step} step and {sleep_time} sleep time.") + self.log.info( + f"{self.hexapod_name} starts loop with {step} step and {sleep_time} sleep time." + ) # Move to the origin first + self.log.info(f"Move the {self.hexapod_name} to the origin") await self._move_to_origin() # Positive direction - self.log.debug("Move from 0 to maximum position") + self.log.info(f"Move {self.hexapod_name} from 0 to maximum position") await self.move_stepwise( 0.0, self.config.max_position, @@ -366,7 +403,7 @@ async def single_loop(self, step, sleep_time): sleep_time, ) - self.log.debug("Move from maximum position back to 0") + self.log.info(f"Move {self.hexapod_name} from maximum position back to 0") await self.move_stepwise( self.config.max_position, 0.0, @@ -375,7 +412,7 @@ async def single_loop(self, step, sleep_time): ) # Negative direction - self.log.debug("Move from 0 to minimum position") + self.log.info(f"Move {self.hexapod_name} from 0 to minimum position") await self.move_stepwise( 0.0, -self.config.max_position, @@ -383,7 +420,7 @@ async def single_loop(self, step, sleep_time): sleep_time, ) - self.log.debug("Move from minimum position back to 0") + self.log.info(f"Move {self.hexapod_name} from minimum position back to 0") await self.move_stepwise( -self.config.max_position, 0.0, @@ -414,5 +451,5 @@ async def _move_to_origin(self, max_count: int = 5) -> None: count += 1 raise RuntimeError( - f"Failed to move the hexapod to the origin. with {max_count} tries" + f"Failed to move {self.hexapod_name} to the origin. with {max_count} tries" ) From d501a76e6eeea9ce2b09756d01a53c20eb3d449b Mon Sep 17 00:00:00 2001 From: Te-Wei Tsai Date: Wed, 22 Jan 2025 11:11:40 -0300 Subject: [PATCH 3/8] Expect the salobj.base.AckError. --- python/lsst/ts/externalscripts/maintel/warmup_hexapod.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py b/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py index 58a39acea..e4220eeb5 100644 --- a/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py +++ b/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py @@ -349,7 +349,7 @@ async def _move_hexapod( await self.move_hexapod(x, y, z, u, v, w=w) return True - except (asyncio.CancelledError, TimeoutError): + except (asyncio.CancelledError, TimeoutError, salobj.base.AckError): self.log.exception( f"Error moving the {self.hexapod_name} to {x=}, {y=}, {z=}, {u=}, {v=}." ) From 8244dfa5d066d2ecab8ba80fe4fbb9dce2eae18a Mon Sep 17 00:00:00 2001 From: Te-Wei Date: Wed, 22 Jan 2025 11:43:42 -0300 Subject: [PATCH 4/8] Bypass the possible eerror in recovering. --- .../externalscripts/maintel/warmup_hexapod.py | 28 ++++++++++++------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py b/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py index e4220eeb5..80c87d47a 100644 --- a/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py +++ b/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py @@ -355,18 +355,26 @@ async def _move_hexapod( ) # If the hexapod is moving, stop it - controller_enabled_state = ( - self.hexapod.evt_controllerState.get().enabledSubstate - ) - if controller_enabled_state == EnabledSubstate.MOVING_POINT_TO_POINT: - self.log.info(f"Stop the {self.hexapod_name} CSC.") - await self.hexapod.cmd_stop.set_start() + try: + controller_enabled_state = ( + self.hexapod.evt_controllerState.get().enabledSubstate + ) + if controller_enabled_state == EnabledSubstate.MOVING_POINT_TO_POINT: + self.log.info(f"Stop the {self.hexapod_name} CSC.") + await self.hexapod.cmd_stop.set_start() + except Exception: + pass # If the hexapod is in fault, recover it - state = self.hexapod.evt_summaryState.get() - if state == salobj.State.FAULT: - self.log.info(f"Recover the {self.hexapod_name} CSC from the Fault.") - await salobj.set_summary_state(self.hexapod, salobj.State.ENABLED) + try: + state = self.hexapod.evt_summaryState.get() + if state == salobj.State.FAULT: + self.log.info( + f"Recover the {self.hexapod_name} CSC from the Fault." + ) + await salobj.set_summary_state(self.hexapod, salobj.State.ENABLED) + except Exception: + pass # Wait for a few seconds await asyncio.sleep(5.0) From b09c1e2b912519e1a4ebb3e34027a0b9430f3558 Mon Sep 17 00:00:00 2001 From: Te-Wei Date: Wed, 22 Jan 2025 11:50:09 -0300 Subject: [PATCH 5/8] Swap the order --- .../externalscripts/maintel/warmup_hexapod.py | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py b/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py index 80c87d47a..ab499d473 100644 --- a/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py +++ b/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py @@ -354,17 +354,6 @@ async def _move_hexapod( f"Error moving the {self.hexapod_name} to {x=}, {y=}, {z=}, {u=}, {v=}." ) - # If the hexapod is moving, stop it - try: - controller_enabled_state = ( - self.hexapod.evt_controllerState.get().enabledSubstate - ) - if controller_enabled_state == EnabledSubstate.MOVING_POINT_TO_POINT: - self.log.info(f"Stop the {self.hexapod_name} CSC.") - await self.hexapod.cmd_stop.set_start() - except Exception: - pass - # If the hexapod is in fault, recover it try: state = self.hexapod.evt_summaryState.get() @@ -376,6 +365,17 @@ async def _move_hexapod( except Exception: pass + # If the hexapod is moving, stop it + try: + controller_enabled_state = ( + self.hexapod.evt_controllerState.get().enabledSubstate + ) + if controller_enabled_state == EnabledSubstate.MOVING_POINT_TO_POINT: + self.log.info(f"Stop the {self.hexapod_name} CSC.") + await self.hexapod.cmd_stop.set_start() + except Exception: + pass + # Wait for a few seconds await asyncio.sleep(5.0) From 1c8d16a6640135edca8dbab6a098d8e53a344f43 Mon Sep 17 00:00:00 2001 From: Te-Wei Date: Wed, 22 Jan 2025 11:58:32 -0300 Subject: [PATCH 6/8] fix the syntax --- python/lsst/ts/externalscripts/maintel/warmup_hexapod.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py b/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py index ab499d473..50f33ebaa 100644 --- a/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py +++ b/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py @@ -356,7 +356,7 @@ async def _move_hexapod( # If the hexapod is in fault, recover it try: - state = self.hexapod.evt_summaryState.get() + state = self.hexapod.evt_summaryState.get().summaryState if state == salobj.State.FAULT: self.log.info( f"Recover the {self.hexapod_name} CSC from the Fault." From 47e8af181c19c28697e335bc48aed69b9e1e17ab Mon Sep 17 00:00:00 2001 From: Te-Wei Date: Wed, 22 Jan 2025 12:20:51 -0300 Subject: [PATCH 7/8] remove the try except --- .../externalscripts/maintel/warmup_hexapod.py | 28 +++++++------------ 1 file changed, 10 insertions(+), 18 deletions(-) diff --git a/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py b/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py index 50f33ebaa..d5a671132 100644 --- a/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py +++ b/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py @@ -355,26 +355,18 @@ async def _move_hexapod( ) # If the hexapod is in fault, recover it - try: - state = self.hexapod.evt_summaryState.get().summaryState - if state == salobj.State.FAULT: - self.log.info( - f"Recover the {self.hexapod_name} CSC from the Fault." - ) - await salobj.set_summary_state(self.hexapod, salobj.State.ENABLED) - except Exception: - pass + state = self.hexapod.evt_summaryState.get().summaryState + if state == salobj.State.FAULT: + self.log.info(f"Recover the {self.hexapod_name} CSC from the Fault.") + await salobj.set_summary_state(self.hexapod, salobj.State.ENABLED) # If the hexapod is moving, stop it - try: - controller_enabled_state = ( - self.hexapod.evt_controllerState.get().enabledSubstate - ) - if controller_enabled_state == EnabledSubstate.MOVING_POINT_TO_POINT: - self.log.info(f"Stop the {self.hexapod_name} CSC.") - await self.hexapod.cmd_stop.set_start() - except Exception: - pass + controller_enabled_state = ( + self.hexapod.evt_controllerState.get().enabledSubstate + ) + if controller_enabled_state == EnabledSubstate.MOVING_POINT_TO_POINT: + self.log.info(f"Stop the {self.hexapod_name} CSC.") + await self.hexapod.cmd_stop.set_start() # Wait for a few seconds await asyncio.sleep(5.0) From 19549698a390ba269dd33abb8311a7e0a69d5484 Mon Sep 17 00:00:00 2001 From: Te-Wei Date: Wed, 22 Jan 2025 12:30:03 -0300 Subject: [PATCH 8/8] Change the default step size. --- python/lsst/ts/externalscripts/maintel/warmup_hexapod.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py b/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py index d5a671132..bd57744e1 100644 --- a/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py +++ b/python/lsst/ts/externalscripts/maintel/warmup_hexapod.py @@ -97,7 +97,7 @@ def get_schema(cls): anyOf: - type: number - type: array - default: 250 + default: 100 sleep_time: description: >- The sleep time in seconds between movements. The number of