diff --git a/robohive/robot/hardware_base.py b/robohive/robot/hardware_base.py index 6b003d69..4397b69f 100644 --- a/robohive/robot/hardware_base.py +++ b/robohive/robot/hardware_base.py @@ -11,6 +11,29 @@ class hardwareBase(abc.ABC): + + # add tests to all defined subclasses to ensure that get_sensors() returns a dict with a 'time' key + def __init_subclass__(cls, **kwargs): + super().__init_subclass__(**kwargs) + if 'connect' in cls.__dict__: + user_connect = cls.__dict__['connect'] + + def connect_then_check(self, *args, **kw): + result = user_connect(self, *args, **kw) + try: + data = self.get_sensors() + if not (isinstance(data, dict) and 'time' in data): + warnings.warn( + f"{self.name}: get_sensors() should return a dict containing a 'time' key, got {type(data)}" + ) + except Exception as e: + warnings.warn( + f"{self.name}: could not verify get_sensors() 'time'-key contract after connect: {e}" + ) + return result + + cls.connect = connect_then_check + def __init__(self, name, *args, **kwargs) -> None: self.name = name @@ -20,7 +43,11 @@ def connect(self) -> bool: @abc.abstractmethod def okay(self) -> bool: - """Return hardware health""" + """Check if hardware is healthy and return the status""" + + @abc.abstractmethod + def recover(self) -> None: + """Recover hardware from any error, connection loss, failure, etc """ @abc.abstractmethod def close(self) -> bool: @@ -28,20 +55,11 @@ def close(self) -> bool: @abc.abstractmethod def reset(self) -> None: - """Reset hardware""" + """Reset hardware to a known state. Used for resetting the hardware to a known state""" @abc.abstractmethod - def _get_sensors(self) -> dict: - """Get hardware sensors — returned dict must include a 'time' key""" - def get_sensors(self) -> dict: - """Get hardware sensors, enforcing 'time' key contract""" - data = self._get_sensors() - if not (isinstance(data, dict) and 'time' in data): - warnings.warn( - f"{self.name}: get_sensors() should return a dict containing a 'time' key, got {type(data)}. " - "Please add 'time' details to your sensor data to suppress this warning.") - return data + """Get hardware sensors — should return a dict containing a 'time' key""" @abc.abstractmethod def apply_commands(self) -> None: diff --git a/robohive/robot/hardware_dynamixel.py b/robohive/robot/hardware_dynamixel.py index 16c99c55..6c769f18 100644 --- a/robohive/robot/hardware_dynamixel.py +++ b/robohive/robot/hardware_dynamixel.py @@ -38,7 +38,7 @@ def close(self): def reset(self): """Reset hardware""" - def _get_sensors(self): + def get_sensors(self): """Get hardware sensors""" def apply_commands(self): diff --git a/robohive/robot/hardware_franka.py b/robohive/robot/hardware_franka.py index 7a2b8f07..cdba1a10 100644 --- a/robohive/robot/hardware_franka.py +++ b/robohive/robot/hardware_franka.py @@ -185,7 +185,7 @@ def reset(self, reset_pos=None, time_to_go=5): self.reset(reset_pos, time_to_go) - def _get_sensors(self) -> dict: + def get_sensors(self) -> dict: """Get hardware sensors""" try: joint_pos = self.robot.get_joint_positions() @@ -193,7 +193,7 @@ def _get_sensors(self) -> dict: except: print("Failed to get current sensors: ", end="") self.reconnect() - return self._get_sensors() + return self.get_sensors() return {'time': time.time(), 'joint_pos': joint_pos, 'joint_vel': joint_vel} diff --git a/robohive/robot/hardware_optitrack.py b/robohive/robot/hardware_optitrack.py index 5aab551b..31106f3a 100644 --- a/robohive/robot/hardware_optitrack.py +++ b/robohive/robot/hardware_optitrack.py @@ -134,7 +134,7 @@ def __repr__(self): self.data_float[2], self.data_float[3], self.data_float[4]) # get latest sensor value (helpful when there is a single sensors) - def _get_sensors(self) -> dict: + def get_sensors(self) -> dict: # sensor_data isn't updated in place ==> it can be easily passed around and cached # repeated calls will return the same data_frame ==> no overhead for multiple queries to the same sensor reading return self.sensor_data diff --git a/robohive/robot/hardware_realsense.py b/robohive/robot/hardware_realsense.py index faa68e5f..17c0541c 100644 --- a/robohive/robot/hardware_realsense.py +++ b/robohive/robot/hardware_realsense.py @@ -58,7 +58,7 @@ def callback(self, pkt): timestamp_str_wo_nano = timestamp_str[:23] + timestamp_str[29:] self.most_recent_pkt_ts = datetime.datetime.fromisoformat(timestamp_str_wo_nano) - def _get_sensors(self) -> dict: + def get_sensors(self) -> dict: # get all data from all topics last_img = copy.deepcopy(self.last_image_pkt) last_depth = copy.deepcopy(self.last_depth_pkt) diff --git a/robohive/robot/hardware_robotiq.py b/robohive/robot/hardware_robotiq.py index 76182c8d..689568e6 100644 --- a/robohive/robot/hardware_robotiq.py +++ b/robohive/robot/hardware_robotiq.py @@ -88,14 +88,14 @@ def reset(self, width=None, **kwargs): self.apply_commands(width=width, **kwargs) - def _get_sensors(self) -> dict: + def get_sensors(self) -> dict: """Get hardware sensors""" try: curr_state = self.robot.get_state() except: print("RBQ:> Failed to get current sensors: ", end="") self.reconnect() - return self._get_sensors() + return self.get_sensors() return {'time': time.time(), 'width': np.array([curr_state.width])} def apply_commands(self, width:float, speed:float=0.1, force:float=0.1):