From e4bd73cb154b17e6e761cbb009cee7d8794a419a Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Tue, 4 Jun 2024 16:47:59 -0500 Subject: [PATCH 01/19] WIP: better approach for different types of location/plate config --- pyproject.toml | 1 + src/platecrane_driver/config.py | 28 ++ src/platecrane_driver/plate_resources.json | 16 +- src/platecrane_driver/platecrane_driver.py | 460 ++++++++++++--------- src/platecrane_driver/test.py | 22 +- src/platecrane_driver/types.py | 34 ++ src/platecrane_rest_node.py | 2 +- 7 files changed, 348 insertions(+), 215 deletions(-) create mode 100644 src/platecrane_driver/config.py create mode 100644 src/platecrane_driver/types.py diff --git a/pyproject.toml b/pyproject.toml index 03554eb..e67daeb 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -17,6 +17,7 @@ dependencies = [ "libusb", "pyserial", "ad_sdl.wei", + "pydantic>=2.7", "pytest" ] requires-python = ">=3.8.1" diff --git a/src/platecrane_driver/config.py b/src/platecrane_driver/config.py new file mode 100644 index 0000000..579dd04 --- /dev/null +++ b/src/platecrane_driver/config.py @@ -0,0 +1,28 @@ +from types import Location, PlateResource + + +safe = Location(name="Safe", joint_angles=[182220, 2500, 460, -308]) +stack1 = Location(name="Stack1", joint_angles=[164672, -32623, 472, 5389]) +stack2 = Location(name="Stack2", joint_angles=[182220, -32623, 460, 5420]) +# stack_source_loc = Location(name="Stack_source_loc", joint_angles=[]) +# target_loc = Location(name="target_loc", joint_angles=[]) +stack3 = Location(name="Stack3", joint_angles=[199708, -32623, 514, 5484]) +stack4 = Location(name="Stack4", joint_angles=[217401, -32623, 546, 5473]) +stack5 = Location(name="Stack5", joint_angles=[235104, -32623, 532, 5453]) +lidnest1 = Location(name="LidNest1", joint_angles=[168355, -31500, 484, -306]) +lidnest2 = Location(name="LidNest2", joint_angles=[199805, -31500, 484, -306]) +lidnest3 = Location(name="LidNest3", joint_angles=[231449, -31500, 484, -306]) +solo_position_1 = Location(name="Solo.Position1", joint_angles=[36703, -27797, -1000, 3630]) +solo_position_2 = Location(name="Solo.Position2", joint_angles=[53182, -27797, -413, 834]) +solo_position_3 = Location(name="Solo.Position3", joint_angles=[20085, -27000, -8850, 5236]) +solo_position_4 = Location(name="Solo.Position4", joint_angles=[21612, -27209, -8787, 239]) +solo_position_6 = Location(name="Solo.Position6", joint_angles=[48425, -27211, -7818, 2793]) +hidex = Location(name="Hidex.Nest", joint_angles=[102327, -30499, -5855, 2363], safe_approach_height=0) +hidex_safe = Location(name="HidexSafe", joint_angles=[209959, -2500, 490, -262]) +liconic = Location(name="Liconic.Nest", joint_angles=[79498, -28067, -6710, 4099]) +sealer = Location(name="Sealer.Nest", joint_angles=[117468, 1220, -4748, 4550]) +peeler = Location(name="Peeler.Nest", joint_angles=[292611, -30758, -4469, 4257]) + + + +plate96 = PlateResource(plate_height = 0, grip_height=0, plate_height_with_lid=0, lid_height=0, lid_grip_height=0, lid_removal_grip_height=0) diff --git a/src/platecrane_driver/plate_resources.json b/src/platecrane_driver/plate_resources.json index 9d9dc0c..5c10cb1 100644 --- a/src/platecrane_driver/plate_resources.json +++ b/src/platecrane_driver/plate_resources.json @@ -9,11 +9,11 @@ }, "96_well":{ - "plate_above_height": 800, - "plate_pick_steps_module": 1500, - "plate_pick_steps_stack": 2400, + "plate_above_height": 800, + "plate_pick_steps_module": 1700, + "plate_pick_steps_stack": 2700, "plate_lid_steps": 1300, - "lid_destination_height": 1400 + "lid_destination_height": 1300 }, "96_deep_well":{ @@ -49,10 +49,10 @@ "lid_destination_height": 0 }, "test_96_well":{ - "plate_above_height": 800, - "plate_pick_steps_module": 1500, - "plate_pick_steps_stack": 2400, - "plate_lid_steps": 1000, + "plate_above_height": 800, + "plate_pick_steps_module": 1700, + "plate_pick_steps_stack": 1500, + "plate_lid_steps": 1300, "lid_destination_height": 1300 } diff --git a/src/platecrane_driver/platecrane_driver.py b/src/platecrane_driver/platecrane_driver.py index 80c0a08..1c00f18 100644 --- a/src/platecrane_driver/platecrane_driver.py +++ b/src/platecrane_driver/platecrane_driver.py @@ -2,9 +2,10 @@ import json import re from pathlib import Path +#from platecrane_driver.serial_port import SerialPort # use when running through wei/REST clients +from serial_port import SerialPort # use when running through driver -from platecrane_driver.serial_port import SerialPort -#from serial_port import SerialPort +import config class PlateCrane: @@ -32,7 +33,7 @@ def __init__(self, host_path="/dev/ttyUSB2", baud_rate=9600): self.status = 0 self.error = "" self.gripper_length = 0 - self.plate_above_height = 700 + self.plate_above_height = 700 # was 700 # TODO: This seems to change nothing self.plate_pick_steps_stack = 1600 self.plate_pick_steps_module = 1400 self.plate_lid_steps = 800 @@ -252,27 +253,6 @@ def get_position(self) -> list: return current_position - def get_safe_height_jog_steps(self, location: list) -> int: - """Summary - - :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] - :type [ParamName]: [ParamType](, optional) - ... - :raises [ErrorType]: [ErrorDescription] - ... - :return: [ReturnDescription] - :rtype: [ReturnType] - """ - - joint_values = self.get_location_joint_values(location) - current_pos = self.get_position() - - module_safe_height = joint_values[1] + self.plate_above_height - - height_jog_steps = current_pos[1] - module_safe_height - - return height_jog_steps - def set_location( self, location_name: str = "TEMP_0", @@ -495,10 +475,8 @@ def move_joints_neutral(self) -> None: self.move_arm_neutral() self.move_tower_neutral() - def get_module_plate( - self, source: str = None, height_jog_steps: int = 0, height_offset: int = 0 - ) -> None: - """picks up the plate from a module location by moving each joint step by step + def move_nest_approach(self, location: str = None) -> None: + """Moves to the entry location of the location that is given. It moves the R,P and Z joints step by step to aviod collisions. :param source: Name of the source location. :type source: str @@ -507,68 +485,28 @@ def get_module_plate( :raises [PlateCraneLocationException]: [Error for None type locations] :return: None """ - # TODO:Handle the error raising within error_codes.py - if not source: - raise Exception( - "PlateCraneLocationException: NoneType variable is not compatible as a location" - ) - - self.move_single_axis("Y", source) - self.jog("Z", -(self.plate_pick_steps_module - height_offset)) - self.gripper_close() - self.jog("Z", self.plate_pick_steps_module) - def put_module_plate( - self, target: str = None, height_jog_steps: int = 0, height_offset: int = 0 - ) -> None: - """Places the plate onto a module location by moving each joint step by step - - :param target: Name of the target location. - :type target: str - :param height_jog_steps: Number of jogging steps that will be used to move the Z axis to the plate location - :type height_jog_steps: int - :raises [PlateCraneLocationException]: [Error for None type locations] - :return: None - """ - - # TODO:Handle the error raising within error_codes.py - if not target: + if not location: raise Exception( "PlateCraneLocationException: NoneType variable is not compatible as a location" ) - self.move_single_axis("Y", target) - self.jog("Z", -(self.plate_pick_steps_module - height_offset)) - self.gripper_open() - self.jog("Z", self.plate_pick_steps_module) - - def move_module_entry(self, source: str = None, height_jog_steps: int = 0) -> None: - """Moves to the entry location of the location that is given. It moves the R,P and Z joints step by step to aviod collisions. - :param source: Name of the source location. - :type source: str - :param height_jog_steps: Number of jogging steps that will be used to move the Z axis to the plate location - :type height_jog_steps: int - :raises [PlateCraneLocationException]: [Error for None type locations] - :return: None - """ - # TODO:Handle the error raising within error_codes.py + joint_values = self.get_location_joint_values(location) # get joint values of given location + current_pos = self.get_position() # get joint values of current position - if not source: - raise Exception( - "PlateCraneLocationException: NoneType variable is not compatible as a location" - ) + self.plate_above_height = config.location.safe_approach_height + module_safe_height = joint_values[1] + self.plate_above_height - if not height_jog_steps: - height_jog_steps = self.get_safe_height_jog_steps(source) + height_jog_steps = current_pos[1] - module_safe_height # first step of safe approach - self.move_single_axis("R", source) - self.move_single_axis("P", source) + self.move_single_axis("R", location) + self.move_single_axis("P", location) self.jog("Z", -height_jog_steps) - def pick_module_plate( - self, source: str = None, height_jog_steps: int = 0, height_offset: int = 0 + def pick_plate_safe_approach( + self, source: str = None, height_offset: int = 0 ) -> None: """Pick a module plate from a module location. @@ -587,13 +525,16 @@ def pick_module_plate( self.move_joints_neutral() self.gripper_open() - self.move_module_entry(source, height_jog_steps) - self.get_module_plate(source, height_jog_steps, height_offset) + self.move_nest_approach(source) + self.move_single_axis("Y", source) + self.jog("Z", -(self.plate_pick_steps_module - height_offset)) # TODO: change plate_pick_steps_module to plate location height (Z) - module safe height + height offset + self.gripper_close() + self.jog("Z", self.plate_pick_steps_module) self.move_arm_neutral() - def place_module_plate( - self, target: str = None, height_jog_steps: int = 0, height_offset: int = 0 + def place_plate_safe_approach( + self, target: str, height_offset: int = 0 ) -> None: """Place a module plate onto a module location. @@ -604,19 +545,18 @@ def place_module_plate( :raises [PlateCraneLocationException]: [Error for None type locations] :return: None """ - if not target: - raise Exception( - "PlateCraneLocationException: NoneType variable is not compatible as a location" - ) self.move_joints_neutral() - self.move_module_entry(target, height_jog_steps) - self.put_module_plate(target, height_jog_steps, height_offset) + self.move_nest_approach(target) + self.move_single_axis("Y", target) + self.jog("Z", -(self.plate_pick_steps_module - height_offset)) + self.gripper_open() + self.jog("Z", self.plate_pick_steps_module) self.move_arm_neutral() - def pick_stack_plate(self, source: str = None, height_offset: int = 0) -> None: + def pick_plate_direct(self, source: str, source_type: str, height_offset: int = 0, is_lid: bool=False) -> None: """Pick a stack plate from stack location. :param source: Name of the source location. @@ -624,15 +564,11 @@ def pick_stack_plate(self, source: str = None, height_offset: int = 0) -> None: :raises [PlateCraneLocationException]: [Error for None type locations] :return: None """ - if not source: - raise Exception( - "PlateCraneLocationException: NoneType variable is not compatible as a location" - ) self.move_joints_neutral() self.move_single_axis("R", source) - if "stack" in source.lower(): + if source_type == "stack": self.gripper_close() self.move_location(source) self.jog("Z", self.plate_above_height) @@ -641,12 +577,19 @@ def pick_stack_plate(self, source: str = None, height_offset: int = 0) -> None: else: self.gripper_open() self.move_location(source) - # self.jog("Z", -self.plate_pick_steps_stack + height_offset) self.gripper_close() + + if is_lid: + self.jog("Z", 100) + self.jog("Z", 100) + self.jog("Z", 100) + self.jog("Z", 100) + self.jog("Z", 100) + self.move_tower_neutral() self.move_arm_neutral() - def place_stack_plate(self, target: str = None, height_offset: int = 0) -> None: + def place_plate_direct(self, target: str = None, height_offset: int = 0) -> None: """Place a stack plate either onto the exhange location or into a stack :param target: Name of the target location. Defults to None if target is None, it will be set to exchange location. @@ -711,10 +654,6 @@ def remove_lid( """ self.get_new_plate_height(plate_type) - # TESTING - print("LID HEIGHT") - print(self.lid_destination_height) - target_offset = ( 2 * self.plate_above_height - self.plate_pick_steps_stack + self.lid_destination_height # + height_offset @@ -736,8 +675,11 @@ def remove_lid( target=remove_lid_target, source_type="stack", target_type="stack", + incremental_lift = True, ) + + def replace_lid( self, source: str = "Stack2", @@ -783,103 +725,105 @@ def replace_lid( target_type="stack", ) - def stack_transfer( - self, - source: str = None, - target: str = None, - source_type: str = "stack", - target_type: str = "module", - height_offset: int = 0, - ) -> None: - """ - Transfer a plate plate from a plate stack to the exchange location or make a transfer in between stacks and stack entry locations - - :param source: Source location, provided as either a location name or 4 joint values. - :type source: str - :param target: Target location, provided as either a location name or 4 joint values. - :type target: str - :raises [ErrorType]: [ErrorDescription] - :return: None - """ - - if not source or not target: - print("Please provide a source location") - # TODO: Raise an exception here - return - - source = self._is_location_joint_values(location=source, name="source") - target = self._is_location_joint_values(location=target, name="target") - - if source_type.lower() == "stack": - source_loc = self.get_location_joint_values(source) - if "stack" in source.lower(): - stack_source = "stack_source_loc" - source_offset = self.plate_above_height + height_offset - else: - stack_source = "source_loc" - source_offset = ( - 2 * self.plate_above_height - - self.plate_pick_steps_stack - + height_offset - ) - - self.set_location( - stack_source, - source_loc[0], - source_loc[1] + source_offset, - source_loc[2], - source_loc[3], - ) - self.pick_stack_plate(stack_source, height_offset=height_offset) - - elif source_type.lower() == "module": - self.pick_module_plate(source, height_offset=height_offset) - - target_height_jog_steps = self.get_safe_height_jog_steps(target) - if target_type.lower() == "stack": - target_loc = self.get_location_joint_values(target) - target_offset = ( - 2 * self.plate_above_height - - self.plate_pick_steps_stack - + height_offset - ) - stack_target = "target_loc" - self.set_location( - stack_target, - target_loc[0], - target_loc[1] + target_offset, - target_loc[2], - target_loc[3], - ) - self.place_stack_plate(stack_target, height_offset=height_offset) - - elif target_type.lower() == "module": - self.place_module_plate( - target, - height_jog_steps=target_height_jog_steps, - height_offset=height_offset, - ) - - def module_transfer(self, source: str, target: str, height_offset: int = 0) -> None: - """ - Transfer a plate in between two modules using source and target locations - - :param source: Source location, provided as either a location name or 4 joint values. - :type source: str - :param target: Target location, provided as either a location name or 4 joint values. - :type target: str - :raises [ErrorType]: [ErrorDescription] - :return: None - """ - self.move_joints_neutral() - source = self._is_location_joint_values(location=source, name="source") - target = self._is_location_joint_values(location=target, name="target") - - source_height_jog_steps = self.get_safe_height_jog_steps(source) - target_height_jog_steps = self.get_safe_height_jog_steps(target) - - self.pick_module_plate(source, source_height_jog_steps, height_offset) - self.place_module_plate(target, target_height_jog_steps, height_offset) + # def stack_transfer( + # self, + # source: str = None, + # target: str = None, + # source_type: str = "stack", + # target_type: str = "module", + # height_offset: int = 0, + # is_lid: bool = False, + # ) -> None: + # """ + # Transfer a plate plate from a plate stack to the exchange location or make a transfer in between stacks and stack entry locations + + # :param source: Source location, provided as either a location name or 4 joint values. + # :type source: str + # :param target: Target location, provided as either a location name or 4 joint values. + # :type target: str + # :raises [ErrorType]: [ErrorDescription] + # :return: None + # """ + + # if not source or not target: + # print("Please provide a source location") + # # TODO: Raise an exception here + # return + + # source = self._is_location_joint_values(location=source, name="source") + # target = self._is_location_joint_values(location=target, name="target") + + # if source_type.lower() == "stack": + # source_loc = self.get_location_joint_values(source) + # if "stack" in source.lower(): + # stack_source = "stack_source_loc" + # source_offset = self.plate_above_height + height_offset + # else: + # stack_source = "source_loc" + # source_offset = ( + # 2 * self.plate_above_height + # - self.plate_pick_steps_stack + # + height_offset + # ) + + # self.set_location( + # stack_source, + # source_loc[0], + # source_loc[1] + source_offset, + # source_loc[2], + # source_loc[3], + # ) + # self.pick_plate_direct(stack_source, height_offset=height_offset, is_lid=is_lid) + + # elif source_type.lower() == "module": + # self.pick_plate_safe_approach(source, height_offset=height_offset) + + # target_height_jog_steps = self.get_safe_height_jog_steps(target) + # if target_type.lower() == "stack": + # target_loc = self.get_location_joint_values(target) + # target_offset = ( + # 2 * self.plate_above_height + # - self.plate_pick_steps_stack + # + height_offset + # ) + # stack_target = "target_loc" + # self.set_location( + # stack_target, + # target_loc[0], + # target_loc[1] + target_offset, + # target_loc[2], + # target_loc[3], + # ) + # self.place_plate_direct(stack_target, height_offset=height_offset) + + # elif target_type.lower() == "module": + # self.place_plate_safe_approach( + # target, + # height_jog_steps=target_height_jog_steps, + # height_offset=height_offset, + # ) + + # def module_transfer(self, source: str, target: str, height_offset: int = 0) -> None: + # """ + # Transfer a plate in between two modules using source and target locations + + # :param source: Source location, provided as either a location name or 4 joint values. + # :type source: str + # :param target: Target location, provided as either a location name or 4 joint values. + # :type target: str + # :raises [ErrorType]: [ErrorDescription] + # :return: None + # """ + # self.move_joints_neutral() + # source = self._is_location_joint_values(location=source, name="source") + # target = self._is_location_joint_values(location=target, name="target") + + # source_height_jog_steps = self.get_safe_height_jog_steps(source) + # target_height_jog_steps = self.get_safe_height_jog_steps(target) + # print(source_height_jog_steps) + # print(target_height_jog_steps) + # self.pick_plate_safe_approach(source, source_height_jog_steps, height_offset) + # self.place_plate_safe_approach(target, target_height_jog_steps, height_offset) def get_new_plate_height(self, plate_type): """ @@ -922,12 +866,13 @@ def update_stack_resource(self): def transfer( self, - source: str = None, - target: str = None, + source: str, + target: str, source_type: str = "stack", target_type: str = "stack", height_offset: int = 0, plate_type: str = None, + incremental_lift: bool = False, ) -> None: """ Handles the transfer request @@ -943,19 +888,126 @@ def transfer( """ self.get_stack_resource() - if plate_type: self.get_new_plate_height(plate_type) + # if source_type == "stack" or target_type == "stack": + # self.stack_transfer(source, target, source_type, target_type, height_offset, incremental_lift) + # elif source_type == "module" and target_type == "module": + # self.module_transfer(source, target, height_offset) - if source_type == "stack" or target_type == "stack": - self.stack_transfer(source, target, source_type, target_type, height_offset) - elif source_type == "module" and target_type == "module": - self.module_transfer(source, target, height_offset) + + if source_type == "stack": + self.stack_pick(source, height_offset, incremental_lift) + else: + self.nest_pick(source, height_offset, incremental_lift) + if target_type == "stack": + self.stack_place(target, target_type, height_offset, incremental_lift) + else: + self.nest_place(target, target_type, height_offset, incremental_lift) self.move_joints_neutral() self.move_location("Safe") self.update_stack_resource() # + def stack_pick( + self, + source: str = None, + height_offset: int = 0, + incremental_lift: bool = False, + ) -> None: + """ + Transfer a plate plate from a plate stack to the exchange location or make a transfer in between stacks and stack entry locations + + :param source: Source location, provided as either a location name or 4 joint values. + :type source: str + :param target: Target location, provided as either a location name or 4 joint values. + :type target: str + :raises [ErrorType]: [ErrorDescription] + :return: None + """ + + source = self._is_location_joint_values(location=source, name="source") + + + source_loc = self.get_location_joint_values(source) + stack_source = "stack_source_loc" + source_offset = self.plate_above_height + height_offset + + self.set_location( + stack_source, + source_loc[0], + source_loc[1] + source_offset, + source_loc[2], + source_loc[3], + ) + self.pick_plate_direct(stack_source, "stack", height_offset=height_offset, is_lid=incremental_lift) + + def stack_place( + self, + target: str = None, + height_offset: int = 0, + ) -> None: + """ + Transfer a plate plate from a plate stack to the exchange location or make a transfer in between stacks and stack entry locations + + :param source: Source location, provided as either a location name or 4 joint values. + :type source: str + :param target: Target location, provided as either a location name or 4 joint values. + :type target: str + :raises [ErrorType]: [ErrorDescription] + :return: None + """ + + target_loc = self.get_location_joint_values(target) + stack_target = "target_loc" + self.set_location( + stack_target, + target_loc[0], + target_loc[1], + target_loc[2], + target_loc[3], + ) + self.place_plate_direct(stack_target, height_offset=height_offset) + + def nest_pick(self, source: str, height_offset: int = 0) -> None: + """ + Transfer a plate in between two modules using source and target locations + + :param source: Source location, provided as either a location name or 4 joint values. + :type source: str + :param target: Target location, provided as either a location name or 4 joint values. + :type target: str + :raises [ErrorType]: [ErrorDescription] + :return: None + """ + self.move_joints_neutral() + source = self._is_location_joint_values(location=source, name="source") + + + if use_safe_approach: + self.pick_plate_safe_approach(source, height_offset) + else: + self.pick_plate_direct(source, "nest", height_offset) + + def nest_place(self, target: str, height_offset: int = 0) -> None: + """ + Transfer a plate in between two modules using source and target locations + + :param source: Source location, provided as either a location name or 4 joint values. + :type source: str + :param target: Target location, provided as either a location name or 4 joint values. + :type target: str + :raises [ErrorType]: [ErrorDescription] + :return: None + """ + self.move_joints_neutral() + target = self._is_location_joint_values(location=target, name="target") + + if use_safe_approach: + self.place_plate_safe_approach(target, height_offset) + else: + self.place_plate_direct(target, height_offset) + if __name__ == "__main__": """ diff --git a/src/platecrane_driver/test.py b/src/platecrane_driver/test.py index afc79f4..dfc8da8 100644 --- a/src/platecrane_driver/test.py +++ b/src/platecrane_driver/test.py @@ -16,11 +16,29 @@ target_loc = "HidexNest2" lidnest3 = "LidNest3" sealer = "SealerNest" - # s.move_location("Safe") + s.move_location("Safe") # print(s.lid_height) - s.remove_lid(source="Solo.Position2", target="LidNest1", plate_type="test_96_well") + + # s.home() + + + + + print(s.get_location_list()) + #s.set_location("Solo.Position2", 53182, -27797, -413, 834) + # s.set_location("HidexSafe", 209959, -2500, 490, -262) + #s.transfer("Solo.Position2","Solo.Position2",source_type="stack",target_type="stack", plate_type="96_well", height_offset=-200) # works if don't specify plate type (picks up lower) + # s.transfer("Stack1","Solo.Position2",source_type="stack",target_type="stack", plate_type="96_well", height_offset=-250) # works if don't specify plate type (picks up lower) + # s.remove_lid("Solo.Position2", "LidNest1", plate_type="96_well", height_offset=-650) + + # s.transfer("Solo.Position2","Solo.Position1", source_type="stack", target_type="module", plate_type="96_well", height_offset=-250) + #s.transfer("Stack1","Solo.Position2",source_type="stack",target_type="stack", plate_type="96_well") # doesn't work with plate type through driver (picks up higher) + #s.get_position() + + + # s.remove_lid(source="Solo.Position2", target="LidNest1", plate_type="test_96_well", height_offset = -100) # print(s.lid_height) # s.move_location("Solo.Position1") diff --git a/src/platecrane_driver/types.py b/src/platecrane_driver/types.py new file mode 100644 index 0000000..77be10f --- /dev/null +++ b/src/platecrane_driver/types.py @@ -0,0 +1,34 @@ +from typing import List, Optional +from pydantic import BaseModel + +class Location(BaseModel): + + name: str + """Internal name of the location""" + joint_angles: List[int] + """List of 4 joint angles (unit: integer stepper values)""" + safe_approach_height: Optional[int] = None + """A safe height (unit: integer stepper value for Z axis) from which + to extend the arm when approaching this location.""" + + +class PlateResource(BaseModel): + + # Plate Properties + + plate_height: float + """The height measured from the bottom of the plate to the top""" + grip_height: float + """The height at which to grip the plate, measured from the bottom of the plate""" + plate_height_with_lid: Optional[float] = None + """The height of the plate when lidded, measured from the bottom of the plate to the top of the lid. + Only required if the resource supports lids""" + + # Lid Properties + + lid_height: Optional[float] = None + """The height of the lid alone, measured from the bottom of the lid to the top of the lid""" + lid_grip_height: Optional[float] = None + """The height at which to grip the lid itself, measured from the bottom of the lid""" + lid_removal_grip_height: Optional[float] = None + """The height at which to grip the lid when removing it, measured from the bottom of the lidded plate""" diff --git a/src/platecrane_rest_node.py b/src/platecrane_rest_node.py index b01b8a5..3c93421 100644 --- a/src/platecrane_rest_node.py +++ b/src/platecrane_rest_node.py @@ -218,7 +218,7 @@ def do_action(action_handle: str, action_vars): print("Source location: " + str(source)) target = action_args.get("target") print("Target location: " + str(target)) - plate_type = action_args.get("plate_type", "96_well") + plate_type = action_args.get("plate_type", "96_well") print("Plate type: " + str(target)) height_offset = action_args.get("height_offset", 0) print("Height Offset: " + str(height_offset)) From 0861776b2f7099236cd0721bc1bf36caf78e652e Mon Sep 17 00:00:00 2001 From: abestroka Date: Wed, 5 Jun 2024 16:00:30 -0500 Subject: [PATCH 02/19] references config file instead of platecrane memory, adjusts how movements are made --- src/platecrane_driver/config.py | 38 ++-- src/platecrane_driver/platecrane_driver.py | 234 +++++++-------------- 2 files changed, 101 insertions(+), 171 deletions(-) diff --git a/src/platecrane_driver/config.py b/src/platecrane_driver/config.py index 579dd04..02443e7 100644 --- a/src/platecrane_driver/config.py +++ b/src/platecrane_driver/config.py @@ -1,28 +1,30 @@ from types import Location, PlateResource -safe = Location(name="Safe", joint_angles=[182220, 2500, 460, -308]) -stack1 = Location(name="Stack1", joint_angles=[164672, -32623, 472, 5389]) -stack2 = Location(name="Stack2", joint_angles=[182220, -32623, 460, 5420]) +safe = Location(name="Safe", joint_angles=[182220, 2500, 460, -308], safe_approach_height=0) +stack1 = Location(name="Stack1", joint_angles=[164672, -32623, 472, 5389], safe_approach_height=0) +stack2 = Location(name="Stack2", joint_angles=[182220, -32623, 460, 5420], safe_approach_height=0) # stack_source_loc = Location(name="Stack_source_loc", joint_angles=[]) # target_loc = Location(name="target_loc", joint_angles=[]) -stack3 = Location(name="Stack3", joint_angles=[199708, -32623, 514, 5484]) -stack4 = Location(name="Stack4", joint_angles=[217401, -32623, 546, 5473]) -stack5 = Location(name="Stack5", joint_angles=[235104, -32623, 532, 5453]) -lidnest1 = Location(name="LidNest1", joint_angles=[168355, -31500, 484, -306]) -lidnest2 = Location(name="LidNest2", joint_angles=[199805, -31500, 484, -306]) -lidnest3 = Location(name="LidNest3", joint_angles=[231449, -31500, 484, -306]) -solo_position_1 = Location(name="Solo.Position1", joint_angles=[36703, -27797, -1000, 3630]) -solo_position_2 = Location(name="Solo.Position2", joint_angles=[53182, -27797, -413, 834]) -solo_position_3 = Location(name="Solo.Position3", joint_angles=[20085, -27000, -8850, 5236]) -solo_position_4 = Location(name="Solo.Position4", joint_angles=[21612, -27209, -8787, 239]) -solo_position_6 = Location(name="Solo.Position6", joint_angles=[48425, -27211, -7818, 2793]) +stack3 = Location(name="Stack3", joint_angles=[199708, -32623, 514, 5484], safe_approach_height=0) +stack4 = Location(name="Stack4", joint_angles=[217401, -32623, 546, 5473], safe_approach_height=0) +stack5 = Location(name="Stack5", joint_angles=[235104, -32623, 532, 5453], safe_approach_height=0) +lidnest1 = Location(name="LidNest1", joint_angles=[168355, -31500, 484, -306], safe_approach_height=0) +lidnest2 = Location(name="LidNest2", joint_angles=[199805, -31500, 484, -306], safe_approach_height=0) +lidnest3 = Location(name="LidNest3", joint_angles=[231449, -31500, 484, -306], safe_approach_height=0) +solo_position_1 = Location(name="Solo.Position1", joint_angles=[36703, -27797, -1000, 3630], safe_approach_height=0) +solo_position_2 = Location(name="Solo.Position2", joint_angles=[53182, -27797, -413, 834], safe_approach_height=0) +solo_position_3 = Location(name="Solo.Position3", joint_angles=[20085, -27000, -8850, 5236], safe_approach_height=0) +solo_position_4 = Location(name="Solo.Position4", joint_angles=[21612, -27209, -8787, 239], safe_approach_height=0) +solo_position_6 = Location(name="Solo.Position6", joint_angles=[48425, -27211, -7818, 2793], safe_approach_height=0) hidex = Location(name="Hidex.Nest", joint_angles=[102327, -30499, -5855, 2363], safe_approach_height=0) -hidex_safe = Location(name="HidexSafe", joint_angles=[209959, -2500, 490, -262]) -liconic = Location(name="Liconic.Nest", joint_angles=[79498, -28067, -6710, 4099]) -sealer = Location(name="Sealer.Nest", joint_angles=[117468, 1220, -4748, 4550]) -peeler = Location(name="Peeler.Nest", joint_angles=[292611, -30758, -4469, 4257]) +hidex_safe = Location(name="HidexSafe", joint_angles=[209959, -2500, 490, -262], safe_approach_height=0) +liconic = Location(name="Liconic.Nest", joint_angles=[79498, -28067, -6710, 4099], safe_approach_height=0) +sealer = Location(name="Sealer.Nest", joint_angles=[117468, 1220, -4748, 4550], safe_approach_height=0) +peeler = Location(name="Peeler.Nest", joint_angles=[292611, -30758, -4469, 4257], safe_approach_height=0) plate96 = PlateResource(plate_height = 0, grip_height=0, plate_height_with_lid=0, lid_height=0, lid_grip_height=0, lid_removal_grip_height=0) +tipbox = PlateResource(plate_height = 0, grip_height=0, plate_height_with_lid=0, lid_height=0, lid_grip_height=0, lid_removal_grip_height=0) +platepcr = PlateResource(plate_height = 0, grip_height=0, plate_height_with_lid=0, lid_height=0, lid_grip_height=0, lid_removal_grip_height=0) diff --git a/src/platecrane_driver/platecrane_driver.py b/src/platecrane_driver/platecrane_driver.py index 1c00f18..b02ac96 100644 --- a/src/platecrane_driver/platecrane_driver.py +++ b/src/platecrane_driver/platecrane_driver.py @@ -493,16 +493,21 @@ def move_nest_approach(self, location: str = None) -> None: ) - joint_values = self.get_location_joint_values(location) # get joint values of given location + # get joint values of given location + joint_values = config.location.joint_angles current_pos = self.get_position() # get joint values of current position self.plate_above_height = config.location.safe_approach_height module_safe_height = joint_values[1] + self.plate_above_height + self.module_safe_height = module_safe_height height_jog_steps = current_pos[1] - module_safe_height # first step of safe approach - self.move_single_axis("R", location) - self.move_single_axis("P", location) + self.move_joint_angles(R=config.location.joint_angles[0], Z=current_pos[1], P=current_pos[2], Y=current_pos[3]) + # self.move_single_axis("R", location) # TODO: adjust to use config instead of values stored in platecrane + current_pos = self.get_position() # get joint values of current position + self.move_joint_angles(R=current_pos[0], Z=current_pos[1], P=config.location.joint_angles[2], Y=current_pos[3]) + # self.move_single_axis("P", location) self.jog("Z", -height_jog_steps) def pick_plate_safe_approach( @@ -521,15 +526,18 @@ def pick_plate_safe_approach( raise Exception( "PlateCraneLocationException: NoneType variable is not compatible as a location" ) - self.move_joints_neutral() self.gripper_open() self.move_nest_approach(source) - self.move_single_axis("Y", source) - self.jog("Z", -(self.plate_pick_steps_module - height_offset)) # TODO: change plate_pick_steps_module to plate location height (Z) - module safe height + height offset + current_pos = self.get_position() + self.move_joint_angles(R=current_pos[0], Z=current_pos[1], P=current_pos[2], Y=config.source.joint_angless[3]) + + # self.move_single_axis("Y", source) + # self.jog("Z", -(self.plate_pick_steps_module - height_offset)) + self.jog("Z", (config.source.joint_angles[1] - self.module_safe_height + height_offset)) #TODO: test, replace height offset self.gripper_close() - self.jog("Z", self.plate_pick_steps_module) + self.jog("Z", config.source.safe_approach_height) self.move_arm_neutral() @@ -548,11 +556,14 @@ def place_plate_safe_approach( self.move_joints_neutral() - self.move_nest_approach(target) + self.move_nest_approach(target) # faces destination, lowers to safe approach height + current_pos = self.get_position() + self.move_joint_angles(R=current_pos[0], Z=current_pos[1], P=current_pos[2], Y=config.source.joint_angless[3]) self.move_single_axis("Y", target) - self.jog("Z", -(self.plate_pick_steps_module - height_offset)) + # self.jog("Z", -(self.plate_pick_steps_module - height_offset)) + self.jog("Z", (config.target.joint_angles[1] - self.module_safe_height + height_offset)) #TODO: test, replace height offsetwith plate dimensions self.gripper_open() - self.jog("Z", self.plate_pick_steps_module) + self.jog("Z", config.target.safe_approach_height) self.move_arm_neutral() @@ -566,17 +577,21 @@ def pick_plate_direct(self, source: str, source_type: str, height_offset: int = """ self.move_joints_neutral() - self.move_single_axis("R", source) + current_pos = self.get_position() + self.move_joint_angles(R=config.source.joint_angles[0], Z=current_pos[1], P=current_pos[2], Y=current_pos[3]) + # self.move_single_axis("R", source) if source_type == "stack": self.gripper_close() - self.move_location(source) - self.jog("Z", self.plate_above_height) + # self.move_location(source) + self.move_joint_angles(R=config.source.joint_angles[0], Z=config.source.joint_angles[1], P=config.source.joint_angles[2], Y=config.source.joint_angles[3]) + self.jog("Z", 1000) # height moved up after tap TODO: make constant value? 1000? self.gripper_open() - self.jog("Z", -self.plate_pick_steps_stack + height_offset) + self.jog("Z", -1000 + height_offset) # move same distance back down, plus size of plate TODO: add plate dimensions else: self.gripper_open() - self.move_location(source) + self.move_joint_angles(R=config.source.joint_angles[0], Z=config.source.joint_angles[1], P=config.source.joint_angles[2], Y=config.source.joint_angles[3]) + # self.move_location(source) self.gripper_close() if is_lid: @@ -586,7 +601,7 @@ def pick_plate_direct(self, source: str, source_type: str, height_offset: int = self.jog("Z", 100) self.jog("Z", 100) - self.move_tower_neutral() + self.move_tower_neutral()# TODO: need to change to use config? self.move_arm_neutral() def place_plate_direct(self, target: str = None, height_offset: int = 0) -> None: @@ -596,12 +611,14 @@ def place_plate_direct(self, target: str = None, height_offset: int = 0) -> None :type target: str :return: None """ - - self.move_joints_neutral() - self.move_single_axis("R", target) - self.move_location(target) + current_pos = self.get_position() + self.move_joints_neutral() # TODO change to config + # self.move_single_axis("R", target) + self.move_joint_angles(R=config.target.joint_angles[0], Z=current_pos[1], P=current_pos[2], Y=current_pos[3]) + self.move_joint_angles(R=config.target.joint_angles[0], Z=config.target.joint_angles[1], P=config.target.joint_angles[2], Y=config.target.joint_angles[3]) + # self.move_location(target) self.gripper_open() - self.move_tower_neutral() + self.move_tower_neutral() # TODO: change self.move_arm_neutral() def _is_location_joint_values(self, location: str, name: str = "temp") -> str: @@ -618,7 +635,8 @@ def _is_location_joint_values(self, location: str, name: str = "temp") -> str: :rtype: str """ try: - location = eval(location) + # location = eval(location) # replacing with checking config + from config import location except NameError: # Location was given as a location name print(name + ": " + location) @@ -652,7 +670,9 @@ def remove_lid( :raises [ErrorType]: [ErrorDescription] :return: None """ - self.get_new_plate_height(plate_type) + # self.get_new_plate_height(plate_type) # pulls plate dimenstions, now do from config + #TODO: pull plate dimenstions? + plate_height = config.plate_type.plate_height target_offset = ( 2 * self.plate_above_height - self.plate_pick_steps_stack + self.lid_destination_height @@ -725,105 +745,6 @@ def replace_lid( target_type="stack", ) - # def stack_transfer( - # self, - # source: str = None, - # target: str = None, - # source_type: str = "stack", - # target_type: str = "module", - # height_offset: int = 0, - # is_lid: bool = False, - # ) -> None: - # """ - # Transfer a plate plate from a plate stack to the exchange location or make a transfer in between stacks and stack entry locations - - # :param source: Source location, provided as either a location name or 4 joint values. - # :type source: str - # :param target: Target location, provided as either a location name or 4 joint values. - # :type target: str - # :raises [ErrorType]: [ErrorDescription] - # :return: None - # """ - - # if not source or not target: - # print("Please provide a source location") - # # TODO: Raise an exception here - # return - - # source = self._is_location_joint_values(location=source, name="source") - # target = self._is_location_joint_values(location=target, name="target") - - # if source_type.lower() == "stack": - # source_loc = self.get_location_joint_values(source) - # if "stack" in source.lower(): - # stack_source = "stack_source_loc" - # source_offset = self.plate_above_height + height_offset - # else: - # stack_source = "source_loc" - # source_offset = ( - # 2 * self.plate_above_height - # - self.plate_pick_steps_stack - # + height_offset - # ) - - # self.set_location( - # stack_source, - # source_loc[0], - # source_loc[1] + source_offset, - # source_loc[2], - # source_loc[3], - # ) - # self.pick_plate_direct(stack_source, height_offset=height_offset, is_lid=is_lid) - - # elif source_type.lower() == "module": - # self.pick_plate_safe_approach(source, height_offset=height_offset) - - # target_height_jog_steps = self.get_safe_height_jog_steps(target) - # if target_type.lower() == "stack": - # target_loc = self.get_location_joint_values(target) - # target_offset = ( - # 2 * self.plate_above_height - # - self.plate_pick_steps_stack - # + height_offset - # ) - # stack_target = "target_loc" - # self.set_location( - # stack_target, - # target_loc[0], - # target_loc[1] + target_offset, - # target_loc[2], - # target_loc[3], - # ) - # self.place_plate_direct(stack_target, height_offset=height_offset) - - # elif target_type.lower() == "module": - # self.place_plate_safe_approach( - # target, - # height_jog_steps=target_height_jog_steps, - # height_offset=height_offset, - # ) - - # def module_transfer(self, source: str, target: str, height_offset: int = 0) -> None: - # """ - # Transfer a plate in between two modules using source and target locations - - # :param source: Source location, provided as either a location name or 4 joint values. - # :type source: str - # :param target: Target location, provided as either a location name or 4 joint values. - # :type target: str - # :raises [ErrorType]: [ErrorDescription] - # :return: None - # """ - # self.move_joints_neutral() - # source = self._is_location_joint_values(location=source, name="source") - # target = self._is_location_joint_values(location=target, name="target") - - # source_height_jog_steps = self.get_safe_height_jog_steps(source) - # target_height_jog_steps = self.get_safe_height_jog_steps(target) - # print(source_height_jog_steps) - # print(target_height_jog_steps) - # self.pick_plate_safe_approach(source, source_height_jog_steps, height_offset) - # self.place_plate_safe_approach(target, target_height_jog_steps, height_offset) def get_new_plate_height(self, plate_type): """ @@ -873,6 +794,7 @@ def transfer( height_offset: int = 0, plate_type: str = None, incremental_lift: bool = False, + use_safe_approach: bool = False ) -> None: """ Handles the transfer request @@ -887,9 +809,10 @@ def transfer( :return: None """ - self.get_stack_resource() + # self.get_stack_resource() # doesn't do anything if plate_type: - self.get_new_plate_height(plate_type) + # self.get_new_plate_height(plate_type) # TODO: replace with pulling plate dimenstions from config (or just do later as needed), for now just putting plate type in global + self.plate_type = plate_type # if source_type == "stack" or target_type == "stack": # self.stack_transfer(source, target, source_type, target_type, height_offset, incremental_lift) # elif source_type == "module" and target_type == "module": @@ -899,15 +822,16 @@ def transfer( if source_type == "stack": self.stack_pick(source, height_offset, incremental_lift) else: - self.nest_pick(source, height_offset, incremental_lift) + self.nest_pick(source, height_offset, incremental_lift, use_safe_approach) if target_type == "stack": self.stack_place(target, target_type, height_offset, incremental_lift) else: - self.nest_place(target, target_type, height_offset, incremental_lift) + self.nest_place(target, target_type, height_offset, incremental_lift, use_safe_approach) self.move_joints_neutral() - self.move_location("Safe") - self.update_stack_resource() # + self.move_joint_angles(R=config.safe.joint_angles[0], Z=config.safe.joint_angles[1], P=config.safe.joint_angles[2], Y=config.safe.joint_angles[3]) + + # self.update_stack_resource() #TODO: does nothing, could be nice preventing slamming def stack_pick( self, @@ -926,21 +850,23 @@ def stack_pick( :return: None """ - source = self._is_location_joint_values(location=source, name="source") + # source = self._is_location_joint_values(location=source, name="source") # TODO: removed coordinate functionality for simplicity + # block here sets source location as new location in platecrane, can remove + # source_loc = self.get_location_joint_values(source) + # stack_source = "stack_source_loc" + # source_offset = self.plate_above_height + height_offset - source_loc = self.get_location_joint_values(source) - stack_source = "stack_source_loc" - source_offset = self.plate_above_height + height_offset + # self.set_location( + # stack_source, + # source_loc[0], + # source_loc[1] + source_offset, + # source_loc[2], + # source_loc[3], + # ) + # self.pick_plate_direct(stack_source, "stack", height_offset=height_offset, is_lid=incremental_lift) + self.pick_plate_direct(source, "stack", height_offset=height_offset, is_lid=incremental_lift) - self.set_location( - stack_source, - source_loc[0], - source_loc[1] + source_offset, - source_loc[2], - source_loc[3], - ) - self.pick_plate_direct(stack_source, "stack", height_offset=height_offset, is_lid=incremental_lift) def stack_place( self, @@ -958,18 +884,20 @@ def stack_place( :return: None """ - target_loc = self.get_location_joint_values(target) - stack_target = "target_loc" - self.set_location( - stack_target, - target_loc[0], - target_loc[1], - target_loc[2], - target_loc[3], - ) - self.place_plate_direct(stack_target, height_offset=height_offset) - - def nest_pick(self, source: str, height_offset: int = 0) -> None: + # target_loc = self.get_location_joint_values(target) + # stack_target = "target_loc" + # self.set_location( + # stack_target, + # target_loc[0], + # target_loc[1], + # target_loc[2], + # target_loc[3], + # ) + # self.place_plate_direct(stack_target, height_offset=height_offset) + self.place_plate_direct(target, height_offset=height_offset) + + + def nest_pick(self, source: str, height_offset: int = 0, use_safe_approach: bool = False) -> None: """ Transfer a plate in between two modules using source and target locations @@ -989,7 +917,7 @@ def nest_pick(self, source: str, height_offset: int = 0) -> None: else: self.pick_plate_direct(source, "nest", height_offset) - def nest_place(self, target: str, height_offset: int = 0) -> None: + def nest_place(self, target: str, height_offset: int = 0, use_safe_approach: bool = False) -> None: """ Transfer a plate in between two modules using source and target locations From 95df4713747b386aa511c6b29a0799901957a237 Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Thu, 6 Jun 2024 17:42:05 -0500 Subject: [PATCH 03/19] progress towards rewrite, trnasfer, stack pick, and pick plate direct generally done --- src/platecrane_driver/config.py | 30 --- src/platecrane_driver/platecrane_driver.py | 255 +++++++++++++-------- src/platecrane_driver/resource_defs.py | 30 +++ src/platecrane_driver/resource_types.py | 43 ++++ src/platecrane_rest_node.py | 22 +- 5 files changed, 246 insertions(+), 134 deletions(-) delete mode 100644 src/platecrane_driver/config.py create mode 100644 src/platecrane_driver/resource_defs.py create mode 100644 src/platecrane_driver/resource_types.py diff --git a/src/platecrane_driver/config.py b/src/platecrane_driver/config.py deleted file mode 100644 index 02443e7..0000000 --- a/src/platecrane_driver/config.py +++ /dev/null @@ -1,30 +0,0 @@ -from types import Location, PlateResource - - -safe = Location(name="Safe", joint_angles=[182220, 2500, 460, -308], safe_approach_height=0) -stack1 = Location(name="Stack1", joint_angles=[164672, -32623, 472, 5389], safe_approach_height=0) -stack2 = Location(name="Stack2", joint_angles=[182220, -32623, 460, 5420], safe_approach_height=0) -# stack_source_loc = Location(name="Stack_source_loc", joint_angles=[]) -# target_loc = Location(name="target_loc", joint_angles=[]) -stack3 = Location(name="Stack3", joint_angles=[199708, -32623, 514, 5484], safe_approach_height=0) -stack4 = Location(name="Stack4", joint_angles=[217401, -32623, 546, 5473], safe_approach_height=0) -stack5 = Location(name="Stack5", joint_angles=[235104, -32623, 532, 5453], safe_approach_height=0) -lidnest1 = Location(name="LidNest1", joint_angles=[168355, -31500, 484, -306], safe_approach_height=0) -lidnest2 = Location(name="LidNest2", joint_angles=[199805, -31500, 484, -306], safe_approach_height=0) -lidnest3 = Location(name="LidNest3", joint_angles=[231449, -31500, 484, -306], safe_approach_height=0) -solo_position_1 = Location(name="Solo.Position1", joint_angles=[36703, -27797, -1000, 3630], safe_approach_height=0) -solo_position_2 = Location(name="Solo.Position2", joint_angles=[53182, -27797, -413, 834], safe_approach_height=0) -solo_position_3 = Location(name="Solo.Position3", joint_angles=[20085, -27000, -8850, 5236], safe_approach_height=0) -solo_position_4 = Location(name="Solo.Position4", joint_angles=[21612, -27209, -8787, 239], safe_approach_height=0) -solo_position_6 = Location(name="Solo.Position6", joint_angles=[48425, -27211, -7818, 2793], safe_approach_height=0) -hidex = Location(name="Hidex.Nest", joint_angles=[102327, -30499, -5855, 2363], safe_approach_height=0) -hidex_safe = Location(name="HidexSafe", joint_angles=[209959, -2500, 490, -262], safe_approach_height=0) -liconic = Location(name="Liconic.Nest", joint_angles=[79498, -28067, -6710, 4099], safe_approach_height=0) -sealer = Location(name="Sealer.Nest", joint_angles=[117468, 1220, -4748, 4550], safe_approach_height=0) -peeler = Location(name="Peeler.Nest", joint_angles=[292611, -30758, -4469, 4257], safe_approach_height=0) - - - -plate96 = PlateResource(plate_height = 0, grip_height=0, plate_height_with_lid=0, lid_height=0, lid_grip_height=0, lid_removal_grip_height=0) -tipbox = PlateResource(plate_height = 0, grip_height=0, plate_height_with_lid=0, lid_height=0, lid_grip_height=0, lid_removal_grip_height=0) -platepcr = PlateResource(plate_height = 0, grip_height=0, plate_height_with_lid=0, lid_height=0, lid_grip_height=0, lid_removal_grip_height=0) diff --git a/src/platecrane_driver/platecrane_driver.py b/src/platecrane_driver/platecrane_driver.py index b02ac96..9ed1e3e 100644 --- a/src/platecrane_driver/platecrane_driver.py +++ b/src/platecrane_driver/platecrane_driver.py @@ -2,10 +2,11 @@ import json import re from pathlib import Path + #from platecrane_driver.serial_port import SerialPort # use when running through wei/REST clients from serial_port import SerialPort # use when running through driver - -import config +from resource_defs import locations, plate_definitions +#import platecrane_driver.resource_defs as resource_defs class PlateCrane: @@ -33,11 +34,11 @@ def __init__(self, host_path="/dev/ttyUSB2", baud_rate=9600): self.status = 0 self.error = "" self.gripper_length = 0 - self.plate_above_height = 700 # was 700 # TODO: This seems to change nothing - self.plate_pick_steps_stack = 1600 - self.plate_pick_steps_module = 1400 - self.plate_lid_steps = 800 - self.lid_destination_height = 1400 + # self.plate_above_height = 700 # was 700 # TODO: This seems to change nothing + # self.plate_pick_steps_stack = 1600 + # self.plate_pick_steps_module = 1400 + # self.plate_lid_steps = 800 + # self.lid_destination_height = 1400 self.stack_exchange_Z_height = -31887 self.stack_exchange_Y_axis_steps = 200 # TODO: Find the correct number of steps to move Y axis from the stack to the exchange location @@ -90,44 +91,44 @@ def home(self, timeout=28): command = "HOME\r\n" self.__serial_port.send_command(command, timeout) - def get_robot_movement_state(self): - """Summary - - :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] - :type [ParamName]: [ParamType](, optional) - ... - :raises [ErrorType]: [ErrorDescription] - ... - :return: [ReturnDescription] - :rtype: [ReturnType] - """ - - # current_postion = self.get_position() - # print(current_postion) - # print(self.platecrane_current_position) - # if self.platecrane_current_position != current_postion: - # self.movement_state = "BUSY" - # self.platecrane_current_position = current_postion - # else: - # self.movement_state = "READY" - # print(self.movement_state) - self.movement_state = "READY" - - def wait_robot_movement(self): - """Summary - - :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] - :type [ParamName]: [ParamType](, optional) - ... - :raises [ErrorType]: [ErrorDescription] - ... - :return: [ReturnDescription] - :rtype: [ReturnType] - """ - - self.get_robot_movement_state() - if self.movement_state != "READY": - self.wait_robot_movement() + # def get_robot_movement_state(self): # NEVER USED + # """Summary + + # :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] + # :type [ParamName]: [ParamType](, optional) + # ... + # :raises [ErrorType]: [ErrorDescription] + # ... + # :return: [ReturnDescription] + # :rtype: [ReturnType] + # """ + + # # current_postion = self.get_position() + # # print(current_postion) + # # print(self.platecrane_current_position) + # # if self.platecrane_current_position != current_postion: + # # self.movement_state = "BUSY" + # # self.platecrane_current_position = current_postion + # # else: + # # self.movement_state = "READY" + # # print(self.movement_state) + # self.movement_state = "READY" + + # def wait_robot_movement(self): # NEVER USED + # """Summary + + # :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] + # :type [ParamName]: [ParamType](, optional) + # ... + # :raises [ErrorType]: [ErrorDescription] + # ... + # :return: [ReturnDescription] + # :rtype: [ReturnType] + # """ + + # self.get_robot_movement_state() + # if self.movement_state != "READY": + # self.wait_robot_movement() def get_status(self): """Checks status of plate_crane @@ -494,19 +495,19 @@ def move_nest_approach(self, location: str = None) -> None: # get joint values of given location - joint_values = config.location.joint_angles + joint_values = resource_defs.location.joint_angles current_pos = self.get_position() # get joint values of current position - self.plate_above_height = config.location.safe_approach_height + self.plate_above_height = resource_defs.location.safe_approach_height module_safe_height = joint_values[1] + self.plate_above_height self.module_safe_height = module_safe_height height_jog_steps = current_pos[1] - module_safe_height # first step of safe approach - self.move_joint_angles(R=config.location.joint_angles[0], Z=current_pos[1], P=current_pos[2], Y=current_pos[3]) + self.move_joint_angles(R=resource_defs.location.joint_angles[0], Z=current_pos[1], P=current_pos[2], Y=current_pos[3]) # self.move_single_axis("R", location) # TODO: adjust to use config instead of values stored in platecrane current_pos = self.get_position() # get joint values of current position - self.move_joint_angles(R=current_pos[0], Z=current_pos[1], P=config.location.joint_angles[2], Y=current_pos[3]) + self.move_joint_angles(R=current_pos[0], Z=current_pos[1], P=resource_defs.location.joint_angles[2], Y=current_pos[3]) # self.move_single_axis("P", location) self.jog("Z", -height_jog_steps) @@ -531,13 +532,13 @@ def pick_plate_safe_approach( self.move_nest_approach(source) current_pos = self.get_position() - self.move_joint_angles(R=current_pos[0], Z=current_pos[1], P=current_pos[2], Y=config.source.joint_angless[3]) + self.move_joint_angles(R=current_pos[0], Z=current_pos[1], P=current_pos[2], Y=resource_defs.source.joint_angless[3]) # self.move_single_axis("Y", source) # self.jog("Z", -(self.plate_pick_steps_module - height_offset)) - self.jog("Z", (config.source.joint_angles[1] - self.module_safe_height + height_offset)) #TODO: test, replace height offset + self.jog("Z", (resource_defs.source.joint_angles[1] - self.module_safe_height + height_offset)) #TODO: test, replace height offset self.gripper_close() - self.jog("Z", config.source.safe_approach_height) + self.jog("Z", resource_defs.source.safe_approach_height) self.move_arm_neutral() @@ -558,16 +559,16 @@ def place_plate_safe_approach( self.move_nest_approach(target) # faces destination, lowers to safe approach height current_pos = self.get_position() - self.move_joint_angles(R=current_pos[0], Z=current_pos[1], P=current_pos[2], Y=config.source.joint_angless[3]) + self.move_joint_angles(R=current_pos[0], Z=current_pos[1], P=current_pos[2], Y=resource_defs.source.joint_angless[3]) self.move_single_axis("Y", target) # self.jog("Z", -(self.plate_pick_steps_module - height_offset)) - self.jog("Z", (config.target.joint_angles[1] - self.module_safe_height + height_offset)) #TODO: test, replace height offsetwith plate dimensions + self.jog("Z", (resource_defs.target.joint_angles[1] - self.module_safe_height + height_offset)) #TODO: test, replace height offsetwith plate dimensions self.gripper_open() - self.jog("Z", config.target.safe_approach_height) + self.jog("Z", resource_defs.target.safe_approach_height) self.move_arm_neutral() - def pick_plate_direct(self, source: str, source_type: str, height_offset: int = 0, is_lid: bool=False) -> None: + def pick_plate_direct(self, source: str, source_type: str, plate_type: str, height_offset: int = 0, incremental_lift: bool=False) -> None: """Pick a stack plate from stack location. :param source: Name of the source location. @@ -578,23 +579,39 @@ def pick_plate_direct(self, source: str, source_type: str, height_offset: int = self.move_joints_neutral() current_pos = self.get_position() - self.move_joint_angles(R=config.source.joint_angles[0], Z=current_pos[1], P=current_pos[2], Y=current_pos[3]) + self.move_joint_angles( + R=locations[source].joint_angles[0], + Z=current_pos[1], + P=current_pos[2], + Y=current_pos[3], + ) # self.move_single_axis("R", source) if source_type == "stack": self.gripper_close() # self.move_location(source) - self.move_joint_angles(R=config.source.joint_angles[0], Z=config.source.joint_angles[1], P=config.source.joint_angles[2], Y=config.source.joint_angles[3]) - self.jog("Z", 1000) # height moved up after tap TODO: make constant value? 1000? + self.move_joint_angles( + R=locations[source].joint_angles[0], + Z=locations[source].joint_angles[1], + P=locations[source].joint_angles[2], + Y=locations[source].joint_angles[3], + ) + self.jog("Z", 1000) # height moved up after tap self.gripper_open() - self.jog("Z", -1000 + height_offset) # move same distance back down, plus size of plate TODO: add plate dimensions - else: + self.jog("Z", -1000 + plate_definitions[plate_type].grip_height + height_offset) # move down to grab height location + + else: # if source_type == nest: self.gripper_open() - self.move_joint_angles(R=config.source.joint_angles[0], Z=config.source.joint_angles[1], P=config.source.joint_angles[2], Y=config.source.joint_angles[3]) - # self.move_location(source) + self.move_joint_angles( + R=locations[source].joint_angles[0], + Z=locations[source].joint_angles[1] + plate_definitions[plate_type].grip_height, # TODO: this doesn't account for if the trnasfer is a lid..... + P=locations[source].source.joint_angles[2], + Y=locations[source].source.joint_angles[3], + ) + self.gripper_close() - if is_lid: + if incremental_lift: self.jog("Z", 100) self.jog("Z", 100) self.jog("Z", 100) @@ -614,8 +631,8 @@ def place_plate_direct(self, target: str = None, height_offset: int = 0) -> None current_pos = self.get_position() self.move_joints_neutral() # TODO change to config # self.move_single_axis("R", target) - self.move_joint_angles(R=config.target.joint_angles[0], Z=current_pos[1], P=current_pos[2], Y=current_pos[3]) - self.move_joint_angles(R=config.target.joint_angles[0], Z=config.target.joint_angles[1], P=config.target.joint_angles[2], Y=config.target.joint_angles[3]) + self.move_joint_angles(R=resource_defs.target.joint_angles[0], Z=current_pos[1], P=current_pos[2], Y=current_pos[3]) + self.move_joint_angles(R=resource_defs.target.joint_angles[0], Z=resource_defs.target.joint_angles[1], P=resource_defs.target.joint_angles[2], Y=resource_defs.target.joint_angles[3]) # self.move_location(target) self.gripper_open() self.move_tower_neutral() # TODO: change @@ -636,7 +653,7 @@ def _is_location_joint_values(self, location: str, name: str = "temp") -> str: """ try: # location = eval(location) # replacing with checking config - from config import location + from platecrane_driver.resource_defs import location except NameError: # Location was given as a location name print(name + ": " + location) @@ -672,7 +689,7 @@ def remove_lid( """ # self.get_new_plate_height(plate_type) # pulls plate dimenstions, now do from config #TODO: pull plate dimenstions? - plate_height = config.plate_type.plate_height + plate_height = resource_defs.plate_type.plate_height target_offset = ( 2 * self.plate_above_height - self.plate_pick_steps_stack + self.lid_destination_height @@ -789,10 +806,10 @@ def transfer( self, source: str, target: str, - source_type: str = "stack", - target_type: str = "stack", + plate_type: str, + # source_type: str = "stack", + # target_type: str = "stack", height_offset: int = 0, - plate_type: str = None, incremental_lift: bool = False, use_safe_approach: bool = False ) -> None: @@ -809,33 +826,74 @@ def transfer( :return: None """ - # self.get_stack_resource() # doesn't do anything - if plate_type: - # self.get_new_plate_height(plate_type) # TODO: replace with pulling plate dimenstions from config (or just do later as needed), for now just putting plate type in global - self.plate_type = plate_type - # if source_type == "stack" or target_type == "stack": - # self.stack_transfer(source, target, source_type, target_type, height_offset, incremental_lift) - # elif source_type == "module" and target_type == "module": - # self.module_transfer(source, target, height_offset) + # # self.get_stack_resource() # doesn't do anything + # if plate_type: + # # self.get_new_plate_height(plate_type) # TODO: replace with pulling plate dimenstions from config (or just do later as needed), for now just putting plate type in global + # self.plate_type = plate_type + # # if source_type == "stack" or target_type == "stack": + # # self.stack_transfer(source, target, source_type, target_type, height_offset, incremental_lift) + # # elif source_type == "module" and target_type == "module": + # # self.module_transfer(source, target, height_offset) + # Extract the source and target location_types + source_type = locations[source].location_type + target_type = locations[target].location_type + # is safe approach required for source and/or target? + source_use_safe_approach = False if locations[source].safe_approach_height == 0 else True + target_use_safe_approach = False if locations[target].safe_approach_height == 0 else True + + # TESTING + print(source_type) + print(target_type) + + # Grab from source location: if source_type == "stack": - self.stack_pick(source, height_offset, incremental_lift) - else: - self.nest_pick(source, height_offset, incremental_lift, use_safe_approach) + self.stack_pick( + source=source, + source_type=source_type, + plate_type=plate_type, + height_offset=height_offset, + incremental_lift=incremental_lift + ) + elif source_type == "nest": + self.nest_pick( + source=source, + plate_type=plate_type, + height_offset=height_offset, + incremental_lift=incremental_lift, + use_safe_approach=source_use_safe_approach + ) + else: + raise Exception( + "Source location type not defined correctly" + ) + + # Place at target location: if target_type == "stack": - self.stack_place(target, target_type, height_offset, incremental_lift) - else: - self.nest_place(target, target_type, height_offset, incremental_lift, use_safe_approach) + self.stack_place(target, target_type, plate_type, height_offset, incremental_lift) + elif target_type == "nest": + self.nest_place(target, target_type, plate_type, height_offset, incremental_lift, target_use_safe_approach) + else: + raise Exception( + "Target location type not defined correctly" + ) self.move_joints_neutral() - self.move_joint_angles(R=config.safe.joint_angles[0], Z=config.safe.joint_angles[1], P=config.safe.joint_angles[2], Y=config.safe.joint_angles[3]) + self.move_joint_angles( + R=locations["Safe"].joint_angles[0], + Z=locations["Safe"].joint_angles[1], + P=locations["Safe"].joint_angles[2], + Y=locations["Safe"].joint_angles[3] + ) # self.update_stack_resource() #TODO: does nothing, could be nice preventing slamming def stack_pick( self, - source: str = None, + source: str, + source_type: str, + plate_type: str, height_offset: int = 0, incremental_lift: bool = False, ) -> None: @@ -865,7 +923,13 @@ def stack_pick( # source_loc[3], # ) # self.pick_plate_direct(stack_source, "stack", height_offset=height_offset, is_lid=incremental_lift) - self.pick_plate_direct(source, "stack", height_offset=height_offset, is_lid=incremental_lift) + self.pick_plate_direct( + source=source, + source_type=source_type, + plate_type=plate_type, + height_offset=height_offset, + incremental_lift=incremental_lift + ) def stack_place( @@ -897,7 +961,7 @@ def stack_place( self.place_plate_direct(target, height_offset=height_offset) - def nest_pick(self, source: str, height_offset: int = 0, use_safe_approach: bool = False) -> None: + def nest_pick(self, source: str, plate_type: str, height_offset: int = 0, incremental_lift: bool = False, use_safe_approach: bool = False) -> None: """ Transfer a plate in between two modules using source and target locations @@ -909,13 +973,18 @@ def nest_pick(self, source: str, height_offset: int = 0, use_safe_approach: bool :return: None """ self.move_joints_neutral() - source = self._is_location_joint_values(location=source, name="source") - + #source = self._is_location_joint_values(location=source, name="source") if use_safe_approach: - self.pick_plate_safe_approach(source, height_offset) + self.pick_plate_safe_approach(source, plate_type, height_offset) else: - self.pick_plate_direct(source, "nest", height_offset) + self.pick_plate_direct( + source=source, + source_type="nest", + plate_type=plate_type, + height_offset=height_offset, + incremental_lift=incremental_lift, + ) def nest_place(self, target: str, height_offset: int = 0, use_safe_approach: bool = False) -> None: """ diff --git a/src/platecrane_driver/resource_defs.py b/src/platecrane_driver/resource_defs.py new file mode 100644 index 0000000..048ce50 --- /dev/null +++ b/src/platecrane_driver/resource_defs.py @@ -0,0 +1,30 @@ +from resource_types import Location, PlateResource +#from platecrane_driver.resource_types import Location, PlateResource + +# Locations accessible by the PlateCrane EX +locations = { + "Safe": Location(name="Safe", joint_angles=[182220, 2500, 460, -308], location_type="nest", safe_approach_height=0), + "Stack1": Location(name="Stack1", joint_angles=[164672, -32623, 472, 5389], location_type="stack", safe_approach_height=0), + "Stack2": Location(name="Stack2", joint_angles=[182220, -32623, 460, 5420], location_type="stack", safe_approach_height=0), + "Stack3": Location(name="Stack3", joint_angles=[199708, -32623, 514, 5484], location_type="stack", safe_approach_height=0), + "Stack4": Location(name="Stack4", joint_angles=[217401, -32623, 546, 5473], location_type="stack", safe_approach_height=0), + "Stack5": Location(name="Stack5", joint_angles=[235104, -32623, 532, 5453], location_type="stack", safe_approach_height=0), + "LidNest1": Location(name="LidNest1", joint_angles=[168355, -31500, 484, -306], location_type="nest", safe_approach_height=0), + "LidNest2": Location(name="LidNest2", joint_angles=[199805, -31500, 484, -306], location_type="nest", safe_approach_height=0), + "LidNest3": Location(name="LidNest3", joint_angles=[231449, -31500, 484, -306], location_type="nest", safe_approach_height=0), + "Solo.Position1": Location(name="Solo.Position1", joint_angles=[36703, -27797, -1000, 3630], location_type="nest", safe_approach_height=0), + "Solo.Position2": Location(name="Solo.Position2", joint_angles=[53182, -27797, -413, 834], location_type="nest", safe_approach_height=0), + "Hidex.Nest": Location(name="Hidex.Nest", joint_angles=[102327, -30499, -5855, 2363], location_type="nest", safe_approach_height=0), + "Liconic.Nest": Location(name="Liconic.Nest", joint_angles=[79498, -28067, -6710, 4099], location_type="nest", safe_approach_height=0), + "Sealer.Nest": Location(name="Sealer.Nest", joint_angles=[117468, 1220, -4748, 4550], location_type="nest", safe_approach_height=0), + "Peeler.Nest": Location(name="Peeler.Nest", joint_angles=[292611, -30758, -4469, 4257], location_type="nest", safe_approach_height=0), +} + + +# Dimensions of labware used on the BIO_Workcell +plate_definitions = { + "flat_bottom_96well": PlateResource(plate_height = 14, grip_height=3, plate_height_with_lid=16, lid_height=10, lid_grip_height=4, lid_removal_grip_height=12), + "tip_box_180uL": PlateResource(plate_height = 0, grip_height=0, plate_height_with_lid=0, lid_height=0, lid_grip_height=0, lid_removal_grip_height=0), + "pcr_96well": PlateResource(plate_height = 0, grip_height=0, plate_height_with_lid=0, lid_height=0, lid_grip_height=0, lid_removal_grip_height=0), +} + diff --git a/src/platecrane_driver/resource_types.py b/src/platecrane_driver/resource_types.py new file mode 100644 index 0000000..cc393a9 --- /dev/null +++ b/src/platecrane_driver/resource_types.py @@ -0,0 +1,43 @@ +from typing import List, Optional +from pydantic import BaseModel + +class Location(BaseModel): + + name: str + """Internal name of the location""" + joint_angles: List[int] + """List of 4 joint angles (unit: integer stepper values)""" + location_type: str + """Type of location, either stack or nest. This will be used to determine gripper path for interactions with the location""" + safe_approach_height: Optional[int] = None + """A safe height (unit: integer stepper value for Z axis) from which + to extend the arm when approaching this location.""" + + +class PlateResource(BaseModel): + + # Plate Properties + + plate_height: float + """The height measured from the bottom of the plate to the top""" + grip_height: float + """The height at which to grip the plate, measured from the bottom of the plate""" + plate_height_with_lid: Optional[float] = None + """The height of the plate when lidded, measured from the bottom of the plate to the top of the lid. + Only required if the resource supports lids""" + + # Lid Properties + + lid_height: Optional[float] = None + """The height of the lid alone, measured from the bottom of the lid to the top of the lid""" + lid_grip_height: Optional[float] = None + """The height at which to grip the lid itself, measured from the bottom of the lid""" + lid_removal_grip_height: Optional[float] = None + """The height at which to grip the lid when removing it, measured from the bottom of the lidded plate""" + + def convert_to_steps(plate_measurement_in_mm: float) -> int: + """Converts plate measurements in mm to PlateCrane EX motor steps on the z-axis """ + steps_per_mm = 80.5 + steps = (plate_measurement_in_mm * steps_per_mm).round() + return steps + diff --git a/src/platecrane_rest_node.py b/src/platecrane_rest_node.py index 3c93421..388e91b 100644 --- a/src/platecrane_rest_node.py +++ b/src/platecrane_rest_node.py @@ -218,30 +218,30 @@ def do_action(action_handle: str, action_vars): print("Source location: " + str(source)) target = action_args.get("target") print("Target location: " + str(target)) - plate_type = action_args.get("plate_type", "96_well") - print("Plate type: " + str(target)) + plate_type = action_args.get("plate_type", "flat_bottom_96") + print("Plate type: " + str(plate_type)) height_offset = action_args.get("height_offset", 0) print("Height Offset: " + str(height_offset)) if action_handle == "transfer": print("Starting the transfer request") - source_type = action_args.get("source_type", None) - print("Source Type: " + str(source_type)) + # source_type = action_args.get("source_type", None) + # print("Source Type: " + str(source_type)) - target_type = action_args.get("target_type", None) - print("Target Type: " + str(target_type)) + # target_type = action_args.get("target_type", None) + # print("Target Type: " + str(target_type)) - if not source_type or not target_type: - print("Please provide source and target transfer types!") - state = ModuleStatus.ERROR + # if not source_type or not target_type: + # print("Please provide source and target transfer types!") + # state = ModuleStatus.ERROR try: platecrane.transfer( source, target, - source_type=source_type.lower(), - target_type=target_type.lower(), + # source_type=source_type.lower(), + # target_type=target_type.lower(), height_offset=int(height_offset), plate_type=plate_type, ) From 18f05fc588eb45b00b0f3db82b6125dda03534bd Mon Sep 17 00:00:00 2001 From: Casey Stone Date: Mon, 10 Jun 2024 20:00:03 -0500 Subject: [PATCH 04/19] updated pick transfers, ended at move_nest_approach --- src/platecrane_driver/platecrane_driver.py | 256 +++++++++++++++------ 1 file changed, 181 insertions(+), 75 deletions(-) diff --git a/src/platecrane_driver/platecrane_driver.py b/src/platecrane_driver/platecrane_driver.py index 9ed1e3e..1e740b2 100644 --- a/src/platecrane_driver/platecrane_driver.py +++ b/src/platecrane_driver/platecrane_driver.py @@ -6,6 +6,7 @@ #from platecrane_driver.serial_port import SerialPort # use when running through wei/REST clients from serial_port import SerialPort # use when running through driver from resource_defs import locations, plate_definitions +from resource_types import PlateResource #import platecrane_driver.resource_defs as resource_defs @@ -451,8 +452,16 @@ def move_tower_neutral(self) -> None: :return: None """ + #self.move_single_axis("Z", "Safe", delay_time=1.5) - self.move_single_axis("Z", "Safe", delay_time=1.5) + # TODO: This still creates a TEMP position, moves to it, then deletes it after + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=locations["Safe"].joint_angles[1], + P=current_pos[2], + Y=current_pos[3] + ) def move_arm_neutral(self) -> None: """Moves the arm to neutral position @@ -476,7 +485,10 @@ def move_joints_neutral(self) -> None: self.move_arm_neutral() self.move_tower_neutral() - def move_nest_approach(self, location: str = None) -> None: + def move_nest_approach(self, + location: str, + plate_type: str, + ) -> None: """Moves to the entry location of the location that is given. It moves the R,P and Z joints step by step to aviod collisions. :param source: Name of the source location. @@ -488,10 +500,30 @@ def move_nest_approach(self, location: str = None) -> None: """ # TODO:Handle the error raising within error_codes.py - if not location: - raise Exception( - "PlateCraneLocationException: NoneType variable is not compatible as a location" - ) + # if not location: + # raise Exception( + # "PlateCraneLocationException: NoneType variable is not compatible as a location" + # ) + + # R axis already rotated to location + + # Lower z axis to safe_approach_z height + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=locations[plate_type].safe_approach_height, + P=current_pos[2], + Y=current_pos[3], + ) + + # extend arm above plate + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=current_pos[1], + P=location[location] # TODO add in the p joint values here!!!! + ) + # get joint values of given location @@ -512,7 +544,10 @@ def move_nest_approach(self, location: str = None) -> None: self.jog("Z", -height_jog_steps) def pick_plate_safe_approach( - self, source: str = None, height_offset: int = 0 + self, + source: str, + plate_type: str, + grip_height_in_steps: int, ) -> None: """Pick a module plate from a module location. @@ -523,14 +558,14 @@ def pick_plate_safe_approach( :raises [PlateCraneLocationException]: [Error for None type locations] :return: None """ - if not source: - raise Exception( - "PlateCraneLocationException: NoneType variable is not compatible as a location" - ) + # if not source: + # raise Exception( + # "PlateCraneLocationException: NoneType variable is not compatible as a location" + # ) self.move_joints_neutral() self.gripper_open() - self.move_nest_approach(source) + self.move_nest_approach(source, plate_type) current_pos = self.get_position() self.move_joint_angles(R=current_pos[0], Z=current_pos[1], P=current_pos[2], Y=resource_defs.source.joint_angless[3]) @@ -568,7 +603,16 @@ def place_plate_safe_approach( self.move_arm_neutral() - def pick_plate_direct(self, source: str, source_type: str, plate_type: str, height_offset: int = 0, incremental_lift: bool=False) -> None: + def pick_plate_direct( + self, + source: str, + source_type: str, + plate_type: str, + # height_offset: int = 0, + grip_height_in_steps: int, + has_lid: bool, + incremental_lift: bool=False + ) -> None: """Pick a stack plate from stack location. :param source: Name of the source location. @@ -579,6 +623,8 @@ def pick_plate_direct(self, source: str, source_type: str, plate_type: str, heig self.move_joints_neutral() current_pos = self.get_position() + + # rotate R axis (base rotation) over the plate self.move_joint_angles( R=locations[source].joint_angles[0], Z=current_pos[1], @@ -590,21 +636,44 @@ def pick_plate_direct(self, source: str, source_type: str, plate_type: str, heig if source_type == "stack": self.gripper_close() # self.move_location(source) + + # tap arm on top of plate stack to determine stack height self.move_joint_angles( R=locations[source].joint_angles[0], Z=locations[source].joint_angles[1], P=locations[source].joint_angles[2], Y=locations[source].joint_angles[3], ) - self.jog("Z", 1000) # height moved up after tap + + # move up, open gripper, grab plate at correct height + self.jog("Z", 1000) self.gripper_open() - self.jog("Z", -1000 + plate_definitions[plate_type].grip_height + height_offset) # move down to grab height location + + # calculate z travel from grip height with/without lid + if has_lid: + z_jog_down_from_plate_top = plate_definitions[plate_type].plate_height_with_lid - grip_height_in_steps + else: # does not have lid + z_jog_down_from_plate_top = plate_definitions[plate_type].plate_height - grip_height_in_steps + + # move down to correct z height to grip plate + self.jog("Z", -(1000 + z_jog_down_from_plate_top)) + + + #self.jog("Z", -1000 + plate_definitions[plate_type].grip_height + height_offset) # move down to grab height location + + # current_pos = self.get_position() + # self.move_joint_angles( + # R=current_pos[0], + # Z=current_pos + # ) else: # if source_type == nest: self.gripper_open() + + # TODO: might need to account for if the plate is a lid here? self.move_joint_angles( R=locations[source].joint_angles[0], - Z=locations[source].joint_angles[1] + plate_definitions[plate_type].grip_height, # TODO: this doesn't account for if the trnasfer is a lid..... + Z=locations[source].joint_angles[1] + grip_height_in_steps, P=locations[source].source.joint_angles[2], Y=locations[source].source.joint_angles[3], ) @@ -618,7 +687,7 @@ def pick_plate_direct(self, source: str, source_type: str, plate_type: str, heig self.jog("Z", 100) self.jog("Z", 100) - self.move_tower_neutral()# TODO: need to change to use config? + self.move_tower_neutral() self.move_arm_neutral() def place_plate_direct(self, target: str = None, height_offset: int = 0) -> None: @@ -810,8 +879,10 @@ def transfer( # source_type: str = "stack", # target_type: str = "stack", height_offset: int = 0, + is_lid: bool = False, + has_lid = False, incremental_lift: bool = False, - use_safe_approach: bool = False + # use_safe_approach: bool = False ) -> None: """ Handles the transfer request @@ -839,28 +910,47 @@ def transfer( source_type = locations[source].location_type target_type = locations[target].location_type + # PICK HEIGHT + # Determine pick height from bottom of plate (converted to z motor steps) + if is_lid: + grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].lid_grip + height_offset) + else: # not a lid + grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].grip_height + height_offset) + + # # Define temporary pick location in plate crane memory #TODO: change this so we're never setting new location in the plate crane + # self.set_location( + # "pick_location_temp", + # locations[source].joint_angles[0], + # locations[source].joint_angles[1] + pick_height_in_steps, + # locations[source].joint_angles[2], + # locations[source].joint_angles[3], + # ) + + # # PLACE HEIGHT + # # Define temporary place location based on pick_height_in_steps variable. + # self.set_location + + # is safe approach required for source and/or target? source_use_safe_approach = False if locations[source].safe_approach_height == 0 else True target_use_safe_approach = False if locations[target].safe_approach_height == 0 else True - # TESTING - print(source_type) - print(target_type) - - # Grab from source location: + # Grab from source location if source_type == "stack": self.stack_pick( source=source, source_type=source_type, plate_type=plate_type, - height_offset=height_offset, + grip_height_in_steps=grip_height_in_steps, + has_lid=has_lid, incremental_lift=incremental_lift ) elif source_type == "nest": self.nest_pick( source=source, plate_type=plate_type, - height_offset=height_offset, + #height_offset=height_offset, + grip_height_in_steps=grip_height_in_steps, incremental_lift=incremental_lift, use_safe_approach=source_use_safe_approach ) @@ -894,7 +984,9 @@ def stack_pick( source: str, source_type: str, plate_type: str, - height_offset: int = 0, + # height_offset: int = 0, + grip_height_in_steps: int, + has_lid: bool, incremental_lift: bool = False, ) -> None: """ @@ -927,64 +1019,78 @@ def stack_pick( source=source, source_type=source_type, plate_type=plate_type, - height_offset=height_offset, + # height_offset=height_offset, + grip_height_in_steps=grip_height_in_steps, + has_lid=has_lid, incremental_lift=incremental_lift ) def stack_place( - self, - target: str = None, - height_offset: int = 0, - ) -> None: - """ - Transfer a plate plate from a plate stack to the exchange location or make a transfer in between stacks and stack entry locations + self, + target: str = None, + height_offset: int = 0, + ) -> None: + """ + Transfer a plate plate from a plate stack to the exchange location or make a transfer in between stacks and stack entry locations - :param source: Source location, provided as either a location name or 4 joint values. - :type source: str - :param target: Target location, provided as either a location name or 4 joint values. - :type target: str - :raises [ErrorType]: [ErrorDescription] - :return: None - """ - - # target_loc = self.get_location_joint_values(target) - # stack_target = "target_loc" - # self.set_location( - # stack_target, - # target_loc[0], - # target_loc[1], - # target_loc[2], - # target_loc[3], - # ) - # self.place_plate_direct(stack_target, height_offset=height_offset) - self.place_plate_direct(target, height_offset=height_offset) + :param source: Source location, provided as either a location name or 4 joint values. + :type source: str + :param target: Target location, provided as either a location name or 4 joint values. + :type target: str + :raises [ErrorType]: [ErrorDescription] + :return: None + """ + + # target_loc = self.get_location_joint_values(target) + # stack_target = "target_loc" + # self.set_location( + # stack_target, + # target_loc[0], + # target_loc[1], + # target_loc[2], + # target_loc[3], + # ) + # self.place_plate_direct(stack_target, height_offset=height_offset) + self.place_plate_direct(target, height_offset=height_offset) - def nest_pick(self, source: str, plate_type: str, height_offset: int = 0, incremental_lift: bool = False, use_safe_approach: bool = False) -> None: - """ - Transfer a plate in between two modules using source and target locations + def nest_pick( + self, + source: str, + plate_type: str, + # height_offset: int = 0, + grip_height_in_steps: int, + incremental_lift: bool = False, + use_safe_approach: bool = False + ) -> None: + """ + Transfer a plate in between two modules using source and target locations - :param source: Source location, provided as either a location name or 4 joint values. - :type source: str - :param target: Target location, provided as either a location name or 4 joint values. - :type target: str - :raises [ErrorType]: [ErrorDescription] - :return: None - """ - self.move_joints_neutral() - #source = self._is_location_joint_values(location=source, name="source") - - if use_safe_approach: - self.pick_plate_safe_approach(source, plate_type, height_offset) - else: - self.pick_plate_direct( - source=source, - source_type="nest", - plate_type=plate_type, - height_offset=height_offset, - incremental_lift=incremental_lift, - ) + :param source: Source location, provided as either a location name or 4 joint values. + :type source: str + :param target: Target location, provided as either a location name or 4 joint values. + :type target: str + :raises [ErrorType]: [ErrorDescription] + :return: None + """ + self.move_joints_neutral() + #source = self._is_location_joint_values(location=source, name="source") + + if use_safe_approach: + self.pick_plate_safe_approach( + source=source, + plate_type=plate_type, + grip_height_in_steps=grip_height_in_steps, + ) + else: + self.pick_plate_direct( + source=source, + source_type="nest", + plate_type=plate_type, + grip_height_in_steps=grip_height_in_steps, + incremental_lift=incremental_lift, + ) def nest_place(self, target: str, height_offset: int = 0, use_safe_approach: bool = False) -> None: """ From 65907d59550bea8a55ccafe63d3e1d94dc5cfa4f Mon Sep 17 00:00:00 2001 From: caseystone Date: Tue, 11 Jun 2024 12:40:06 -0500 Subject: [PATCH 05/19] first draft complete --- src/platecrane_driver/platecrane_driver.py | 396 +++++++++++++++------ 1 file changed, 291 insertions(+), 105 deletions(-) diff --git a/src/platecrane_driver/platecrane_driver.py b/src/platecrane_driver/platecrane_driver.py index 1e740b2..3b31877 100644 --- a/src/platecrane_driver/platecrane_driver.py +++ b/src/platecrane_driver/platecrane_driver.py @@ -485,11 +485,93 @@ def move_joints_neutral(self) -> None: self.move_arm_neutral() self.move_tower_neutral() - def move_nest_approach(self, - location: str, + # def move_nest_approach(self, + # location: str, + # plate_type: str, + # grip_height_in_steps: int, + # ) -> None: + # """Moves to the entry location of the location that is given. It moves the R,P and Z joints step by step to aviod collisions. + + # :param source: Name of the source location. + # :type source: str + # :param height_jog_steps: Number of jogging steps that will be used to move the Z axis to the plate location + # :type height_jog_steps: int + # :raises [PlateCraneLocationException]: [Error for None type locations] + # :return: None + # """ + # # TODO:Handle the error raising within error_codes.py + + # # if not location: + # # raise Exception( + # # "PlateCraneLocationException: NoneType variable is not compatible as a location" + # # ) + + # # Rotate base (R axis) toward plate location + # current_pos = self.get_position() + # self.move_joint_angles( + # R=locations[location].joint_angles[0], + # Z=current_pos[1], + # P=current_pos[2], + # Y=current_pos[3], + # ) + + # # Lower z axis to safe_approach_z height + # current_pos = self.get_position() + # self.move_joint_angles( + # R=current_pos[0], + # Z=locations[plate_type].safe_approach_height, + # P=current_pos[2], + # Y=current_pos[3], + # ) + + # # extend arm over plate and rotate gripper to correct orientation + # current_pos = self.get_position() + # self.move_joint_angles( + # R=current_pos[0], + # Z=current_pos[1], + # P=locations[location].joint_angles[2], + # Y=locations[location].joint_angles[3], + # ) + + # # lower arm (z axis) to correct plate grip height + # current_pos = self.get_position() + # self.move_joint_angles( + # R=current_pos[0], + # Z=locations[location].joint_angles[1] + grip_height_in_steps, + # P=current_pos[2], + # Y=current_pos[3], + # ) + + + + + + + + # # get joint values of given location + # joint_values = resource_defs.location.joint_angles + # current_pos = self.get_position() # get joint values of current position + + # self.plate_above_height = resource_defs.location.safe_approach_height + # module_safe_height = joint_values[1] + self.plate_above_height + # self.module_safe_height = module_safe_height + + # height_jog_steps = current_pos[1] - module_safe_height # first step of safe approach + + # self.move_joint_angles(R=resource_defs.location.joint_angles[0], Z=current_pos[1], P=current_pos[2], Y=current_pos[3]) + # # self.move_single_axis("R", location) # TODO: adjust to use config instead of values stored in platecrane + # current_pos = self.get_position() # get joint values of current position + # self.move_joint_angles(R=current_pos[0], Z=current_pos[1], P=resource_defs.location.joint_angles[2], Y=current_pos[3]) + # # self.move_single_axis("P", location) + # self.jog("Z", -height_jog_steps) + + def pick_plate_safe_approach( + self, + source: str, plate_type: str, + grip_height_in_steps: int, ) -> None: - """Moves to the entry location of the location that is given. It moves the R,P and Z joints step by step to aviod collisions. + """Pick a module plate from a module location. :param source: Name of the source location. :type source: str @@ -498,14 +580,19 @@ def move_nest_approach(self, :raises [PlateCraneLocationException]: [Error for None type locations] :return: None """ - # TODO:Handle the error raising within error_codes.py - # if not location: - # raise Exception( - # "PlateCraneLocationException: NoneType variable is not compatible as a location" - # ) + # MOVE TO PICK UP PLATE + self.move_joints_neutral() + self.gripper_open() - # R axis already rotated to location + # Rotate base (R axis) toward plate location + current_pos = self.get_position() + self.move_joint_angles( + R=locations[source].joint_angles[0], + Z=current_pos[1], + P=current_pos[2], + Y=current_pos[3], + ) # Lower z axis to safe_approach_z height current_pos = self.get_position() @@ -516,67 +603,50 @@ def move_nest_approach(self, Y=current_pos[3], ) - # extend arm above plate + # extend arm over plate and rotate gripper to correct orientation current_pos = self.get_position() self.move_joint_angles( R=current_pos[0], Z=current_pos[1], - P=location[location] # TODO add in the p joint values here!!!! + P=locations[source].joint_angles[2], + Y=locations[source].joint_angles[3], ) + # lower arm (z axis) to correct plate grip height + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=locations[source].joint_angles[1] + grip_height_in_steps, + P=current_pos[2], + Y=current_pos[3], + ) + # grip the plate + self.gripper_close() - # get joint values of given location - joint_values = resource_defs.location.joint_angles - current_pos = self.get_position() # get joint values of current position - - self.plate_above_height = resource_defs.location.safe_approach_height - module_safe_height = joint_values[1] + self.plate_above_height - self.module_safe_height = module_safe_height - - height_jog_steps = current_pos[1] - module_safe_height # first step of safe approach - - self.move_joint_angles(R=resource_defs.location.joint_angles[0], Z=current_pos[1], P=current_pos[2], Y=current_pos[3]) - # self.move_single_axis("R", location) # TODO: adjust to use config instead of values stored in platecrane - current_pos = self.get_position() # get joint values of current position - self.move_joint_angles(R=current_pos[0], Z=current_pos[1], P=resource_defs.location.joint_angles[2], Y=current_pos[3]) - # self.move_single_axis("P", location) - self.jog("Z", -height_jog_steps) - - def pick_plate_safe_approach( - self, - source: str, - plate_type: str, - grip_height_in_steps: int, - ) -> None: - """Pick a module plate from a module location. - - :param source: Name of the source location. - :type source: str - :param height_jog_steps: Number of jogging steps that will be used to move the Z axis to the plate location - :type height_jog_steps: int - :raises [PlateCraneLocationException]: [Error for None type locations] - :return: None - """ - # if not source: - # raise Exception( - # "PlateCraneLocationException: NoneType variable is not compatible as a location" - # ) - self.move_joints_neutral() - self.gripper_open() - - self.move_nest_approach(source, plate_type) + # MOVE WITH PLATE SAFELY BACK TO NEUTRAL + # Move arm up to safe approach height current_pos = self.get_position() - self.move_joint_angles(R=current_pos[0], Z=current_pos[1], P=current_pos[2], Y=resource_defs.source.joint_angless[3]) + self.move_joint_angles( + R=current_pos[0], + Z=locations[source].safe_approach_height, + P=current_pos[2], + Y=current_pos[3], + ) - # self.move_single_axis("Y", source) - # self.jog("Z", -(self.plate_pick_steps_module - height_offset)) - self.jog("Z", (resource_defs.source.joint_angles[1] - self.module_safe_height + height_offset)) #TODO: test, replace height offset - self.gripper_close() - self.jog("Z", resource_defs.source.safe_approach_height) + # retract arm (Y axis) as much as possible (to same Y axis value as Safe location) + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=current_pos[1], + P=current_pos[2], + Y=locations["Safe"].joint_angles[3], + ) + # move rest of joints to neutral location self.move_arm_neutral() + def place_plate_safe_approach( self, target: str, height_offset: int = 0 ) -> None: @@ -674,8 +744,8 @@ def pick_plate_direct( self.move_joint_angles( R=locations[source].joint_angles[0], Z=locations[source].joint_angles[1] + grip_height_in_steps, - P=locations[source].source.joint_angles[2], - Y=locations[source].source.joint_angles[3], + P=locations[source].joint_angles[2], + Y=locations[source].joint_angles[3], ) self.gripper_close() @@ -690,23 +760,53 @@ def pick_plate_direct( self.move_tower_neutral() self.move_arm_neutral() - def place_plate_direct(self, target: str = None, height_offset: int = 0) -> None: + def place_plate_direct( + self, + target: str, + grip_height_in_steps: str, + ) -> None: + """Place a stack plate either onto the exhange location or into a stack :param target: Name of the target location. Defults to None if target is None, it will be set to exchange location. :type target: str :return: None """ - current_pos = self.get_position() - self.move_joints_neutral() # TODO change to config - # self.move_single_axis("R", target) - self.move_joint_angles(R=resource_defs.target.joint_angles[0], Z=current_pos[1], P=current_pos[2], Y=current_pos[3]) - self.move_joint_angles(R=resource_defs.target.joint_angles[0], Z=resource_defs.target.joint_angles[1], P=resource_defs.target.joint_angles[2], Y=resource_defs.target.joint_angles[3]) - # self.move_location(target) - self.gripper_open() - self.move_tower_neutral() # TODO: change + self.move_arm_neutral() + # Rotate base (R axis) to target location + current_pos = self.get_position() + self.move_joint_angles( + R=locations[target].joint_angles[0], + Z=current_pos[1], + P=current_pos[2], + Y=current_pos[3], + ) + + # Extend arm over plate location (Y axis) and rotate gripper to correct orientation (P axis) + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=current_pos[1], + P=locations[target].joint_angles[2], + Y=locations[target].joint_angles[3], + ) + + # Lower arm (z axis) to plate grip height + current_pos=self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=locations[target].joint_angles[1] + grip_height_in_steps, + P=current_pos[2], + Y=current_pos[3], + ) + + self.gripper_close() + + self.move_tower_neutral() + self.move_joints_neutral() + def _is_location_joint_values(self, location: str, name: str = "temp") -> str: """ If the location was provided as joint values, transfer joint values into a saved location on the robot and return the location name. @@ -952,23 +1052,34 @@ def transfer( #height_offset=height_offset, grip_height_in_steps=grip_height_in_steps, incremental_lift=incremental_lift, - use_safe_approach=source_use_safe_approach + use_safe_approach=source_use_safe_approach, ) else: raise Exception( "Source location type not defined correctly" ) + - # Place at target location: - if target_type == "stack": - self.stack_place(target, target_type, plate_type, height_offset, incremental_lift) - elif target_type == "nest": - self.nest_place(target, target_type, plate_type, height_offset, incremental_lift, target_use_safe_approach) + # PLACE PLATE + if target_type == "stack": + self.stack_place( + target=target, + grip_height_in_steps=grip_height_in_steps, + ) + elif target_type == "nest": + self.nest_pick( + target=target, + target_type=target_type, + plate_type=plate_type, + grip_height_in_steps=grip_height_in_steps, + use_safe_approach=target_use_safe_approach, + ) else: raise Exception( "Target location type not defined correctly" ) + self.move_joints_neutral() self.move_joint_angles( R=locations["Safe"].joint_angles[0], @@ -1028,8 +1139,8 @@ def stack_pick( def stack_place( self, - target: str = None, - height_offset: int = 0, + target: str, + grip_height_in_steps: str, ) -> None: """ Transfer a plate plate from a plate stack to the exchange location or make a transfer in between stacks and stack entry locations @@ -1042,24 +1153,17 @@ def stack_place( :return: None """ - # target_loc = self.get_location_joint_values(target) - # stack_target = "target_loc" - # self.set_location( - # stack_target, - # target_loc[0], - # target_loc[1], - # target_loc[2], - # target_loc[3], - # ) - # self.place_plate_direct(stack_target, height_offset=height_offset) - self.place_plate_direct(target, height_offset=height_offset) + # TODO: Do we even need this funtion? + self.place_plate_direct( + target=target, + grip_height_in_steps=grip_height_in_steps, + ) def nest_pick( self, source: str, - plate_type: str, - # height_offset: int = 0, + plate_type: str, grip_height_in_steps: int, incremental_lift: bool = False, use_safe_approach: bool = False @@ -1092,26 +1196,108 @@ def nest_pick( incremental_lift=incremental_lift, ) - def nest_place(self, target: str, height_offset: int = 0, use_safe_approach: bool = False) -> None: - """ - Transfer a plate in between two modules using source and target locations + def nest_place( + self, + target: str, + target_type: str, + plate_type: str, + grip_height_in_steps: int, + use_safe_approach: bool = False + ) -> None: + """ + Transfer a plate in between two modules using source and target locations + + :param source: Source location, provided as either a location name or 4 joint values. + :type source: str + :param target: Target location, provided as either a location name or 4 joint values. + :type target: str + :raises [ErrorType]: [ErrorDescription] + :return: None + """ + + # Rotate base (R axix) toward target location + current_pos = self.get_position() + self.move_joint_angles( + R=locations[target].joint_angles[0], + Z=current_pos[1], + P=current_pos[2], + Y=current_pos[3], + ) + + if use_safe_approach: + # Lower z axis to safe_approach_z height + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=locations[plate_type].safe_approach_height, + P=current_pos[2], + Y=current_pos[3], + ) + + # extend arm over plate and rotate gripper to correct orientation + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=current_pos[1], + P=locations[target].joint_angles[2], + Y=locations[target].joint_angles[3], + ) + + # lower arm (z axis) to correct plate grip height + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=locations[target].joint_angles[1] + grip_height_in_steps, + P=current_pos[2], + Y=current_pos[3], + ) - :param source: Source location, provided as either a location name or 4 joint values. - :type source: str - :param target: Target location, provided as either a location name or 4 joint values. - :type target: str - :raises [ErrorType]: [ErrorDescription] - :return: None - """ - self.move_joints_neutral() - target = self._is_location_joint_values(location=target, name="target") + self.gripper_open() - if use_safe_approach: - self.place_plate_safe_approach(target, height_offset) - else: - self.place_plate_direct(target, height_offset) + # Back away using safe approach path + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=locations[target].safe_approach_height, + P=current_pos[2], + Y=current_pos[3], + ) + # retract arm (Y axis) as much as possible (to same Y axis value as Safe location) + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=current_pos[1], + P=current_pos[2], + Y=locations["Safe"].joint_angles[3], + ) + + else: + # extend arm over plate and rotate gripper to correct orientation + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=current_pos[1], + P=locations[target].joint_angles[2], + Y=locations[target].joint_angles[3], + ) + # lower arm (z axis) to correct plate grip height + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=locations[target].joint_angles[1] + grip_height_in_steps, + P=current_pos[2], + Y=current_pos[3], + ) + + self.gripper_open() + + + self.move_tower_neutral() + self.move_arm_neutral() + + if __name__ == "__main__": """ Runs given function. From a926af28b459eb59650bedf84580d2a7ebf9706c Mon Sep 17 00:00:00 2001 From: caseystone Date: Thu, 13 Jun 2024 16:57:13 -0500 Subject: [PATCH 06/19] driver tested, needs to be cleaned --- src/platecrane_driver/platecrane_driver.py | 341 +++++++++++++-------- src/platecrane_driver/resource_defs.py | 12 +- src/platecrane_driver/resource_types.py | 2 +- 3 files changed, 221 insertions(+), 134 deletions(-) diff --git a/src/platecrane_driver/platecrane_driver.py b/src/platecrane_driver/platecrane_driver.py index 3b31877..b96537c 100644 --- a/src/platecrane_driver/platecrane_driver.py +++ b/src/platecrane_driver/platecrane_driver.py @@ -2,14 +2,23 @@ import json import re from pathlib import Path +import time #from platecrane_driver.serial_port import SerialPort # use when running through wei/REST clients from serial_port import SerialPort # use when running through driver from resource_defs import locations, plate_definitions from resource_types import PlateResource + #import platecrane_driver.resource_defs as resource_defs +# TODOs: +# get rid of stack place and nest place, call place_plate_direct +# look into how to slow speed of stack pick and place +# fine tune (z height) of all positions +# check RestNode + + class PlateCrane: """ Description: @@ -249,9 +258,28 @@ def get_position(self) -> list: :rtype: [ReturnType] """ + # time.sleep(10) command = "GETPOS\r\n" - current_position = list(self.__serial_port.send_command(command).split(" ")) - current_position = [eval(x.strip(",")) for x in current_position] + time.sleep(2) + + try: + current_position = list(self.__serial_port.send_command(command).split(" ")) + + print(f"current 1:{current_position}") + current_position = [eval(x.strip(",")) for x in current_position] + + print(f"current 2:{current_position}") + except Exception: + print("Responses overlapping! waiting 5 seconds to resend command") + + time.sleep(5) + current_position = list(self.__serial_port.send_command(command).split(" ")) + + + print(f"current 1:{current_position}") + current_position = [eval(x.strip(",")) for x in current_position] + + print(f"current 2:{current_position}") return current_position @@ -316,6 +344,7 @@ def gripper_open(self): command = "OPEN\r\n" # Command interpreted by Sciclops self.__serial_port.send_command(command) + # time.sleep(2) def gripper_close(self): """Closes gripper @@ -331,6 +360,7 @@ def gripper_close(self): command = "CLOSE\r\n" # Command interpreted by Sciclops self.__serial_port.send_command(command) + # time.sleep(2) def check_open(self): """Checks if gripper is open @@ -403,7 +433,8 @@ def move_joint_angles(self, R: int, Z: int, P: int, Y: int) -> None: self.move_status = "COMPLETED" pass - self.deletepoint("TEMP", R, Z, P, Y) + #self.delete_location("TEMP", R, Z, P, Y) + self.delete_location("TEMP") def move_single_axis(self, axis: str, loc: str, delay_time=1.5) -> None: """Moves on a single axis using an existing location on robot's database @@ -647,29 +678,29 @@ def pick_plate_safe_approach( self.move_arm_neutral() - def place_plate_safe_approach( - self, target: str, height_offset: int = 0 - ) -> None: - """Place a module plate onto a module location. + # def place_plate_safe_approach( + # self, target: str, height_offset: int = 0 + # ) -> None: + # """Place a module plate onto a module location. - :param target: Name of the target location. - :type target: str - :param height_jog_steps: Number of jogging steps that will be used to move the Z axis to the plate location - :type height_jog_steps: int - :raises [PlateCraneLocationException]: [Error for None type locations] - :return: None - """ + # :param target: Name of the target location. + # :type target: str + # :param height_jog_steps: Number of jogging steps that will be used to move the Z axis to the plate location + # :type height_jog_steps: int + # :raises [PlateCraneLocationException]: [Error for None type locations] + # :return: None + # """ - self.move_joints_neutral() + # self.move_joints_neutral() - self.move_nest_approach(target) # faces destination, lowers to safe approach height - current_pos = self.get_position() - self.move_joint_angles(R=current_pos[0], Z=current_pos[1], P=current_pos[2], Y=resource_defs.source.joint_angless[3]) - self.move_single_axis("Y", target) - # self.jog("Z", -(self.plate_pick_steps_module - height_offset)) - self.jog("Z", (resource_defs.target.joint_angles[1] - self.module_safe_height + height_offset)) #TODO: test, replace height offsetwith plate dimensions - self.gripper_open() - self.jog("Z", resource_defs.target.safe_approach_height) + # self.move_nest_approach(target) # faces destination, lowers to safe approach height + # current_pos = self.get_position() + # self.move_joint_angles(R=current_pos[0], Z=current_pos[1], P=current_pos[2], Y=resource_defs.source.joint_angless[3]) + # self.move_single_axis("Y", target) + # # self.jog("Z", -(self.plate_pick_steps_module - height_offset)) + # self.jog("Z", (resource_defs.target.joint_angles[1] - self.module_safe_height + height_offset)) #TODO: test, replace height offsetwith plate dimensions + # self.gripper_open() + # self.jog("Z", resource_defs.target.safe_approach_height) self.move_arm_neutral() @@ -691,6 +722,7 @@ def pick_plate_direct( :return: None """ + print("PICK PLATE DIRECT CALLED") self.move_joints_neutral() current_pos = self.get_position() @@ -721,9 +753,11 @@ def pick_plate_direct( # calculate z travel from grip height with/without lid if has_lid: - z_jog_down_from_plate_top = plate_definitions[plate_type].plate_height_with_lid - grip_height_in_steps + z_jog_down_from_plate_top = PlateResource.convert_to_steps(plate_definitions[plate_type].plate_height_with_lid) - grip_height_in_steps + # z_jog_down_from_plate_top = plate_definitions[plate_type].plate_height_with_lid - grip_height_in_steps else: # does not have lid - z_jog_down_from_plate_top = plate_definitions[plate_type].plate_height - grip_height_in_steps + z_jog_down_from_plate_top = PlateResource.convert_to_steps(plate_definitions[plate_type].plate_height) - grip_height_in_steps + # z_jog_down_from_plate_top = plate_definitions[plate_type].plate_height - grip_height_in_steps # move down to correct z height to grip plate self.jog("Z", -(1000 + z_jog_down_from_plate_top)) @@ -758,8 +792,11 @@ def pick_plate_direct( self.jog("Z", 100) self.move_tower_neutral() + self.move_arm_neutral() + print("PICK PLATE DIRECT FINISHED") + def place_plate_direct( self, target: str, @@ -802,7 +839,7 @@ def place_plate_direct( Y=current_pos[3], ) - self.gripper_close() + self.gripper_open() self.move_tower_neutral() self.move_joints_neutral() @@ -839,9 +876,9 @@ def _is_location_joint_values(self, location: str, name: str = "temp") -> str: def remove_lid( self, - source: str = None, - target: str = "Stack2", - plate_type: str = "96_well", + source: str, + target: str, + plate_type: str, height_offset: int = 0, ) -> None: """ @@ -858,39 +895,44 @@ def remove_lid( """ # self.get_new_plate_height(plate_type) # pulls plate dimenstions, now do from config #TODO: pull plate dimenstions? - plate_height = resource_defs.plate_type.plate_height - - target_offset = ( - 2 * self.plate_above_height - self.plate_pick_steps_stack + self.lid_destination_height - # + height_offset - ) # Finding the correct target hight when only transferring the plate lid - target_loc = self.get_location_joint_values(target) - remove_lid_target = "Temp_Lid_Target_Loc" - - self.set_location( - remove_lid_target, - target_loc[0], - target_loc[1] - target_offset, - target_loc[2], - target_loc[3], - ) - self.plate_pick_steps_stack = self.plate_lid_steps + height_offset + # plate_height = resource_defs.plate_type.plate_height + + # target_offset = ( + # 2 * self.plate_above_height - self.plate_pick_steps_stack + self.lid_destination_height + # # + height_offset + # ) # Finding the correct target hight when only transferring the plate lid + # target_loc = self.get_location_joint_values(target) + # remove_lid_target = "Temp_Lid_Target_Loc" + + # self.set_location( + # remove_lid_target, + # target_loc[0], + # target_loc[1] - target_offset, + # target_loc[2], + # target_loc[3], + # ) + # self.plate_pick_steps_stack = self.plate_lid_steps + height_offset + + source_grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].lid_removal_grip_height + height_offset) + target_grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].plate_height_with_lid - plate_definitions[plate_type].lid_height + height_offset) self.transfer( source=source, - target=remove_lid_target, - source_type="stack", - target_type="stack", + target=target, + plate_type=plate_type, + height_offset=height_offset, + is_lid=True, + source_grip_height_in_steps=source_grip_height_in_steps, + target_grip_height_in_steps=target_grip_height_in_steps, incremental_lift = True, ) - def replace_lid( self, - source: str = "Stack2", - target: str = None, - plate_type: str = "96_well", + source: str, + target: str, + plate_type: str, height_offset: int = 0, ) -> None: """ @@ -906,29 +948,36 @@ def replace_lid( :return: None """ - self.get_new_plate_height(plate_type) + # self.get_new_plate_height(plate_type) - target_offset = ( - 2 * self.plate_above_height - self.plate_pick_steps_stack + self.lid_destination_height - ) + # target_offset = ( + # 2 * self.plate_above_height - self.plate_pick_steps_stack + self.lid_destination_height + # ) - source_loc = self.get_location_joint_values(source) - remove_lid_source = "Temp_Lid_Source_loc" + # source_loc = self.get_location_joint_values(source) + # remove_lid_source = "Temp_Lid_Source_loc" + + # self.set_location( + # remove_lid_source, + # source_loc[0], + # source_loc[1] - target_offset, + # source_loc[2], + # source_loc[3], + # ) + # self.plate_pick_steps_stack = self.plate_lid_steps + height_offset + + source_grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].lid_grip_height + height_offset) + target_grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].lid_removal_grip_height + height_offset) - self.set_location( - remove_lid_source, - source_loc[0], - source_loc[1] - target_offset, - source_loc[2], - source_loc[3], - ) - self.plate_pick_steps_stack = self.plate_lid_steps + height_offset self.transfer( - source=remove_lid_source, + source=source, target=target, - source_type="stack", - target_type="stack", + plate_type=plate_type, + height_offset=height_offset, + source_grip_height_in_steps=source_grip_height_in_steps, + target_grip_height_in_steps=target_grip_height_in_steps, + is_lid=True, ) @@ -976,11 +1025,11 @@ def transfer( source: str, target: str, plate_type: str, - # source_type: str = "stack", - # target_type: str = "stack", - height_offset: int = 0, + height_offset: int = 0, # units = mm is_lid: bool = False, - has_lid = False, + has_lid: bool = False, + source_grip_height_in_steps: int = None, # if iremoving/replacing lid + target_grip_height_in_steps: int = None, # if iremoving/replacing lid incremental_lift: bool = False, # use_safe_approach: bool = False ) -> None: @@ -1012,10 +1061,19 @@ def transfer( # PICK HEIGHT # Determine pick height from bottom of plate (converted to z motor steps) - if is_lid: - grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].lid_grip + height_offset) - else: # not a lid + + if not is_lid: grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].grip_height + height_offset) + source_grip_height_in_steps = grip_height_in_steps + target_grip_height_in_steps = grip_height_in_steps + # if is_lid: + # grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].lid_grip + height_offset) + + # # calculations + + + # else: # not a lid + # grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].grip_height + height_offset) # # Define temporary pick location in plate crane memory #TODO: change this so we're never setting new location in the plate crane # self.set_location( @@ -1034,26 +1092,54 @@ def transfer( # is safe approach required for source and/or target? source_use_safe_approach = False if locations[source].safe_approach_height == 0 else True target_use_safe_approach = False if locations[target].safe_approach_height == 0 else True + print(f"Source safe approach: {source_use_safe_approach}") + print(f"Target safe approach: {target_use_safe_approach}") # Grab from source location if source_type == "stack": - self.stack_pick( - source=source, - source_type=source_type, - plate_type=plate_type, - grip_height_in_steps=grip_height_in_steps, + # self.stack_pick( + # source=source, + # source_type=source_type, + # plate_type=plate_type, + # grip_height_in_steps=grip_height_in_steps, + # has_lid=has_lid, + # incremental_lift=incremental_lift + # ) + self.pick_plate_direct( + source=source, + source_type=source_type, # "stack" + plate_type=plate_type, + grip_height_in_steps=source_grip_height_in_steps, has_lid=has_lid, incremental_lift=incremental_lift ) + elif source_type == "nest": - self.nest_pick( - source=source, - plate_type=plate_type, - #height_offset=height_offset, - grip_height_in_steps=grip_height_in_steps, - incremental_lift=incremental_lift, - use_safe_approach=source_use_safe_approach, - ) + if source_use_safe_approach: + self.pick_plate_safe_approach( + source=source, + plate_type=plate_type, + grip_height_in_steps=source_grip_height_in_steps, + ) + else: + self.pick_plate_direct( + source=source, + source_type=source_type, # nest + plate_type=plate_type, + grip_height_in_steps=source_grip_height_in_steps, + has_lid=has_lid, + incremental_lift=incremental_lift, + ) + + + # self.nest_pick( + # source=source, + # plate_type=plate_type, + # #height_offset=height_offset, + # grip_height_in_steps=grip_height_in_steps, + # incremental_lift=incremental_lift, + # use_safe_approach=source_use_safe_approach, + # ) else: raise Exception( "Source location type not defined correctly" @@ -1064,14 +1150,14 @@ def transfer( if target_type == "stack": self.stack_place( target=target, - grip_height_in_steps=grip_height_in_steps, + grip_height_in_steps=target_grip_height_in_steps, ) elif target_type == "nest": - self.nest_pick( + self.nest_place( target=target, target_type=target_type, plate_type=plate_type, - grip_height_in_steps=grip_height_in_steps, + grip_height_in_steps=target_grip_height_in_steps, use_safe_approach=target_use_safe_approach, ) else: @@ -1160,41 +1246,41 @@ def stack_place( ) - def nest_pick( - self, - source: str, - plate_type: str, - grip_height_in_steps: int, - incremental_lift: bool = False, - use_safe_approach: bool = False - ) -> None: - """ - Transfer a plate in between two modules using source and target locations + # def nest_pick( + # self, + # source: str, + # plate_type: str, + # grip_height_in_steps: int, + # incremental_lift: bool = False, + # use_safe_approach: bool = False + # ) -> None: + # """ + # Transfer a plate in between two modules using source and target locations - :param source: Source location, provided as either a location name or 4 joint values. - :type source: str - :param target: Target location, provided as either a location name or 4 joint values. - :type target: str - :raises [ErrorType]: [ErrorDescription] - :return: None - """ - self.move_joints_neutral() - #source = self._is_location_joint_values(location=source, name="source") + # :param source: Source location, provided as either a location name or 4 joint values. + # :type source: str + # :param target: Target location, provided as either a location name or 4 joint values. + # :type target: str + # :raises [ErrorType]: [ErrorDescription] + # :return: None + # """ + # self.move_joints_neutral() + # #source = self._is_location_joint_values(location=source, name="source") - if use_safe_approach: - self.pick_plate_safe_approach( - source=source, - plate_type=plate_type, - grip_height_in_steps=grip_height_in_steps, - ) - else: - self.pick_plate_direct( - source=source, - source_type="nest", - plate_type=plate_type, - grip_height_in_steps=grip_height_in_steps, - incremental_lift=incremental_lift, - ) + # if use_safe_approach: + # self.pick_plate_safe_approach( + # source=source, + # plate_type=plate_type, + # grip_height_in_steps=grip_height_in_steps, + # ) + # else: + # self.pick_plate_direct( + # source=source, + # source_type="nest", + # plate_type=plate_type, + # grip_height_in_steps=grip_height_in_steps, + # incremental_lift=incremental_lift, + # ) def nest_place( self, @@ -1229,7 +1315,7 @@ def nest_place( current_pos = self.get_position() self.move_joint_angles( R=current_pos[0], - Z=locations[plate_type].safe_approach_height, + Z=locations[target].safe_approach_height, P=current_pos[2], Y=current_pos[3], ) @@ -1251,6 +1337,7 @@ def nest_place( P=current_pos[2], Y=current_pos[3], ) + time.sleep(2) self.gripper_open() diff --git a/src/platecrane_driver/resource_defs.py b/src/platecrane_driver/resource_defs.py index 048ce50..37a9afd 100644 --- a/src/platecrane_driver/resource_defs.py +++ b/src/platecrane_driver/resource_defs.py @@ -12,16 +12,16 @@ "LidNest1": Location(name="LidNest1", joint_angles=[168355, -31500, 484, -306], location_type="nest", safe_approach_height=0), "LidNest2": Location(name="LidNest2", joint_angles=[199805, -31500, 484, -306], location_type="nest", safe_approach_height=0), "LidNest3": Location(name="LidNest3", joint_angles=[231449, -31500, 484, -306], location_type="nest", safe_approach_height=0), - "Solo.Position1": Location(name="Solo.Position1", joint_angles=[36703, -27797, -1000, 3630], location_type="nest", safe_approach_height=0), - "Solo.Position2": Location(name="Solo.Position2", joint_angles=[53182, -27797, -413, 834], location_type="nest", safe_approach_height=0), - "Hidex.Nest": Location(name="Hidex.Nest", joint_angles=[102327, -30499, -5855, 2363], location_type="nest", safe_approach_height=0), - "Liconic.Nest": Location(name="Liconic.Nest", joint_angles=[79498, -28067, -6710, 4099], location_type="nest", safe_approach_height=0), + "Solo.Position1": Location(name="Solo.Position1", joint_angles=[36703, -27951, -1000, 3630], location_type="nest", safe_approach_height=0), + "Solo.Position2": Location(name="Solo.Position2", joint_angles=[53182, -27951, -413, 834], location_type="nest", safe_approach_height=0), + "Hidex.Nest": Location(name="Hidex.Nest", joint_angles=[102327, -31090, -5830, 2378], location_type="nest", safe_approach_height=-27033), "Sealer.Nest": Location(name="Sealer.Nest", joint_angles=[117468, 1220, -4748, 4550], location_type="nest", safe_approach_height=0), "Peeler.Nest": Location(name="Peeler.Nest", joint_angles=[292611, -30758, -4469, 4257], location_type="nest", safe_approach_height=0), + "Liconic.Nest": Location(name="Liconic.Nest", joint_angles=[265603, -19900, -5413, 4953], location_type="nest", safe_approach_height=0), } - -# Dimensions of labware used on the BIO_Workcell +#19831 +# Dimensions of labware used on the BIO_Workcells plate_definitions = { "flat_bottom_96well": PlateResource(plate_height = 14, grip_height=3, plate_height_with_lid=16, lid_height=10, lid_grip_height=4, lid_removal_grip_height=12), "tip_box_180uL": PlateResource(plate_height = 0, grip_height=0, plate_height_with_lid=0, lid_height=0, lid_grip_height=0, lid_removal_grip_height=0), diff --git a/src/platecrane_driver/resource_types.py b/src/platecrane_driver/resource_types.py index cc393a9..d5c798a 100644 --- a/src/platecrane_driver/resource_types.py +++ b/src/platecrane_driver/resource_types.py @@ -38,6 +38,6 @@ class PlateResource(BaseModel): def convert_to_steps(plate_measurement_in_mm: float) -> int: """Converts plate measurements in mm to PlateCrane EX motor steps on the z-axis """ steps_per_mm = 80.5 - steps = (plate_measurement_in_mm * steps_per_mm).round() + steps = int(plate_measurement_in_mm * steps_per_mm) return steps From af10ee86466a0cf9f0f31680fb41e1efc51a6c86 Mon Sep 17 00:00:00 2001 From: caseystone Date: Thu, 13 Jun 2024 17:25:58 -0500 Subject: [PATCH 07/19] client tested, needs cleanup --- src/platecrane_driver/platecrane_driver.py | 13 +++++++++---- src/platecrane_driver/resource_defs.py | 4 ++-- src/platecrane_rest_node.py | 9 +++++---- 3 files changed, 16 insertions(+), 10 deletions(-) diff --git a/src/platecrane_driver/platecrane_driver.py b/src/platecrane_driver/platecrane_driver.py index b96537c..fcd82fd 100644 --- a/src/platecrane_driver/platecrane_driver.py +++ b/src/platecrane_driver/platecrane_driver.py @@ -4,10 +4,15 @@ from pathlib import Path import time -#from platecrane_driver.serial_port import SerialPort # use when running through wei/REST clients -from serial_port import SerialPort # use when running through driver -from resource_defs import locations, plate_definitions -from resource_types import PlateResource +from platecrane_driver.serial_port import SerialPort # use when running through wei/REST clients +from platecrane_driver.resource_defs import locations, plate_definitions +from platecrane_driver.resource_types import PlateResource + +# from serial_port import SerialPort # when running through the drive +# from resource_defs import locations, plate_definitions +# from resource_types import PlateResource + + #import platecrane_driver.resource_defs as resource_defs diff --git a/src/platecrane_driver/resource_defs.py b/src/platecrane_driver/resource_defs.py index 37a9afd..9306015 100644 --- a/src/platecrane_driver/resource_defs.py +++ b/src/platecrane_driver/resource_defs.py @@ -1,5 +1,5 @@ -from resource_types import Location, PlateResource -#from platecrane_driver.resource_types import Location, PlateResource +#from resource_types import Location, PlateResource +from platecrane_driver.resource_types import Location, PlateResource # through WEI # Locations accessible by the PlateCrane EX locations = { diff --git a/src/platecrane_rest_node.py b/src/platecrane_rest_node.py index 388e91b..34d73b2 100644 --- a/src/platecrane_rest_node.py +++ b/src/platecrane_rest_node.py @@ -218,10 +218,12 @@ def do_action(action_handle: str, action_vars): print("Source location: " + str(source)) target = action_args.get("target") print("Target location: " + str(target)) - plate_type = action_args.get("plate_type", "flat_bottom_96") + plate_type = action_args.get("plate_type") print("Plate type: " + str(plate_type)) height_offset = action_args.get("height_offset", 0) print("Height Offset: " + str(height_offset)) + has_lid = action_args.get("has_lid", False) + print("Has Lid: " + str(bool(has_lid))) if action_handle == "transfer": print("Starting the transfer request") @@ -240,10 +242,9 @@ def do_action(action_handle: str, action_vars): platecrane.transfer( source, target, - # source_type=source_type.lower(), - # target_type=target_type.lower(), - height_offset=int(height_offset), plate_type=plate_type, + height_offset=int(height_offset), + has_lid=has_lid, ) except Exception as err: response.action_response = StepStatus.FAILED From a4a4f188bde578386998151ad82507672dbad37b Mon Sep 17 00:00:00 2001 From: Casey Stone Date: Fri, 14 Jun 2024 16:16:54 -0500 Subject: [PATCH 08/19] driver cleanup, phase 1: methods --- src/platecrane_driver/platecrane_driver.py | 770 ++++----------------- 1 file changed, 151 insertions(+), 619 deletions(-) diff --git a/src/platecrane_driver/platecrane_driver.py b/src/platecrane_driver/platecrane_driver.py index fcd82fd..b8cdd87 100644 --- a/src/platecrane_driver/platecrane_driver.py +++ b/src/platecrane_driver/platecrane_driver.py @@ -4,24 +4,20 @@ from pathlib import Path import time -from platecrane_driver.serial_port import SerialPort # use when running through wei/REST clients +from platecrane_driver.serial_port import SerialPort # use when running through WEI REST clients from platecrane_driver.resource_defs import locations, plate_definitions from platecrane_driver.resource_types import PlateResource -# from serial_port import SerialPort # when running through the drive +# from serial_port import SerialPort # use when running through the driver # from resource_defs import locations, plate_definitions # from resource_types import PlateResource - -#import platecrane_driver.resource_defs as resource_defs - - # TODOs: -# get rid of stack place and nest place, call place_plate_direct # look into how to slow speed of stack pick and place # fine tune (z height) of all positions -# check RestNode +# edit all the doc strings to match new functions +# should we be using error_codes.py to be doing some of the error checking/raising class PlateCrane: @@ -48,27 +44,15 @@ def __init__(self, host_path="/dev/ttyUSB2", baud_rate=9600): self.robot_error = "NO ERROR" self.status = 0 self.error = "" - self.gripper_length = 0 - # self.plate_above_height = 700 # was 700 # TODO: This seems to change nothing - # self.plate_pick_steps_stack = 1600 - # self.plate_pick_steps_module = 1400 - # self.plate_lid_steps = 800 - # self.lid_destination_height = 1400 - self.stack_exchange_Z_height = -31887 - self.stack_exchange_Y_axis_steps = 200 # TODO: Find the correct number of steps to move Y axis from the stack to the exchange location - self.exchange_location = "LidNest2" self.robot_status = "" self.movement_state = "READY" self.platecrane_current_position = None - self.plate_resources = json.load( - open(Path(__file__).parent / "plate_resources.json") - ) - self.stack_resources = json.load( - open(Path(__file__).parent / "stack_resources.json") - ) + # self.stack_resources = json.load( + # open(Path(__file__).parent / "stack_resources.json") + # ) self.initialize() @@ -92,8 +76,7 @@ def initialize(self): def home(self, timeout=28): """Homes all of the axes. Returns to neutral position (above exchange) - - :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] + :param timeout: [ParamDescription], defaults to [DefaultParamVal] :type [ParamName]: [ParamType](, optional) ... :raises [ErrorType]: [ErrorDescription] @@ -106,44 +89,6 @@ def home(self, timeout=28): command = "HOME\r\n" self.__serial_port.send_command(command, timeout) - # def get_robot_movement_state(self): # NEVER USED - # """Summary - - # :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] - # :type [ParamName]: [ParamType](, optional) - # ... - # :raises [ErrorType]: [ErrorDescription] - # ... - # :return: [ReturnDescription] - # :rtype: [ReturnType] - # """ - - # # current_postion = self.get_position() - # # print(current_postion) - # # print(self.platecrane_current_position) - # # if self.platecrane_current_position != current_postion: - # # self.movement_state = "BUSY" - # # self.platecrane_current_position = current_postion - # # else: - # # self.movement_state = "READY" - # # print(self.movement_state) - # self.movement_state = "READY" - - # def wait_robot_movement(self): # NEVER USED - # """Summary - - # :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] - # :type [ParamName]: [ParamType](, optional) - # ... - # :raises [ErrorType]: [ErrorDescription] - # ... - # :return: [ReturnDescription] - # :rtype: [ReturnType] - # """ - - # self.get_robot_movement_state() - # if self.movement_state != "READY": - # self.wait_robot_movement() def get_status(self): """Checks status of plate_crane @@ -197,34 +142,6 @@ def get_location_list(self): print("Error in get_status") self.robot_error = err - def __update_locations(self, robot_onboard: list, known_locations: list) -> None: - """Checks the location database on the robot and saves the missing locations to robot onboard - - :param robot_onboard: List of string locations that are saved on the robot - :type robot_onboard: list - - :param known_locations: List of known locations that should exist on robot database - :type known_locations: list - - :return: None - - """ - - for loc in known_locations: - if loc not in robot_onboard: - loc_values = loc.replace(",", "").split( - " " - ) # Removing the ',' caracter - loc_values[0] = loc_values[0][ - loc_values[0].find(":") + 1 : - ] # Removing the index number from the location name - self.set_location( - loc_values[0], - int( - loc_values[1], - int(loc_values[2], int(loc_values[3]), int(loc_values[4])), - ), - ) def get_location_joint_values(self, location: str = None) -> list: """Checks status of plate_crane @@ -263,29 +180,20 @@ def get_position(self) -> list: :rtype: [ReturnType] """ - # time.sleep(10) command = "GETPOS\r\n" - time.sleep(2) + + # time.sleep(2) # helps to reduce serial port overlapping responses try: + # collect coordinates of current position current_position = list(self.__serial_port.send_command(command).split(" ")) - - print(f"current 1:{current_position}") current_position = [eval(x.strip(",")) for x in current_position] - - print(f"current 2:{current_position}") except Exception: - print("Responses overlapping! waiting 5 seconds to resend command") - + print("Overlapping serial responses detected. Waiting 5 seconds to resend latest command") time.sleep(5) current_position = list(self.__serial_port.send_command(command).split(" ")) - - - print(f"current 1:{current_position}") current_position = [eval(x.strip(",")) for x in current_position] - print(f"current 2:{current_position}") - return current_position def set_location( @@ -332,7 +240,7 @@ def delete_location(self, location_name: str = None): command = "DELETEPOINT %s\r\n" % ( location_name - ) # Command interpreted by Sciclops + ) self.__serial_port.send_command(command) def gripper_open(self): @@ -347,10 +255,9 @@ def gripper_open(self): :rtype: [ReturnType] """ - command = "OPEN\r\n" # Command interpreted by Sciclops + command = "OPEN\r\n" self.__serial_port.send_command(command) - # time.sleep(2) - + def gripper_close(self): """Closes gripper @@ -363,9 +270,8 @@ def gripper_close(self): :rtype: [ReturnType] """ - command = "CLOSE\r\n" # Command interpreted by Sciclops + command = "CLOSE\r\n" self.__serial_port.send_command(command) - # time.sleep(2) def check_open(self): """Checks if gripper is open @@ -379,7 +285,7 @@ def check_open(self): :rtype: [ReturnType] """ - command = "GETGRIPPERISOPEN\r\n" # Command interpreted by Sciclops + command = "GETGRIPPERISOPEN\r\n" self.__serial_port.send_command(command) def check_closed(self): @@ -394,7 +300,7 @@ def check_closed(self): :rtype: [ReturnType] """ - command = "GETGRIPPERISCLOSED\r\n" # Command interpreted by Sciclops + command = "GETGRIPPERISCLOSED\r\n" self.__serial_port.send_command(command) def jog(self, axis, distance) -> None: @@ -438,7 +344,6 @@ def move_joint_angles(self, R: int, Z: int, P: int, Y: int) -> None: self.move_status = "COMPLETED" pass - #self.delete_location("TEMP", R, Z, P, Y) self.delete_location("TEMP") def move_single_axis(self, axis: str, loc: str, delay_time=1.5) -> None: @@ -488,8 +393,7 @@ def move_tower_neutral(self) -> None: :return: None """ - #self.move_single_axis("Z", "Safe", delay_time=1.5) - + # TODO: This still creates a TEMP position, moves to it, then deletes it after current_pos = self.get_position() self.move_joint_angles( @@ -504,15 +408,30 @@ def move_arm_neutral(self) -> None: :return: None """ - self.move_single_axis("Y", "Safe", delay_time=1) + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=current_pos[1], + P=current_pos[2], + Y=locations["Safe"].joint_angles[3], + ) def move_gripper_neutral(self) -> None: """Moves the gripper to neutral position :return: None """ + # TODO: this still accesses plate crane intenal safe self.move_single_axis("P", "Safe", delay_time=0.3) + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=current_pos[1], + P=locations, + Y=current_pos[3], + ) + def move_joints_neutral(self) -> None: """Moves all joints neutral position @@ -521,86 +440,6 @@ def move_joints_neutral(self) -> None: self.move_arm_neutral() self.move_tower_neutral() - # def move_nest_approach(self, - # location: str, - # plate_type: str, - # grip_height_in_steps: int, - # ) -> None: - # """Moves to the entry location of the location that is given. It moves the R,P and Z joints step by step to aviod collisions. - - # :param source: Name of the source location. - # :type source: str - # :param height_jog_steps: Number of jogging steps that will be used to move the Z axis to the plate location - # :type height_jog_steps: int - # :raises [PlateCraneLocationException]: [Error for None type locations] - # :return: None - # """ - # # TODO:Handle the error raising within error_codes.py - - # # if not location: - # # raise Exception( - # # "PlateCraneLocationException: NoneType variable is not compatible as a location" - # # ) - - # # Rotate base (R axis) toward plate location - # current_pos = self.get_position() - # self.move_joint_angles( - # R=locations[location].joint_angles[0], - # Z=current_pos[1], - # P=current_pos[2], - # Y=current_pos[3], - # ) - - # # Lower z axis to safe_approach_z height - # current_pos = self.get_position() - # self.move_joint_angles( - # R=current_pos[0], - # Z=locations[plate_type].safe_approach_height, - # P=current_pos[2], - # Y=current_pos[3], - # ) - - # # extend arm over plate and rotate gripper to correct orientation - # current_pos = self.get_position() - # self.move_joint_angles( - # R=current_pos[0], - # Z=current_pos[1], - # P=locations[location].joint_angles[2], - # Y=locations[location].joint_angles[3], - # ) - - # # lower arm (z axis) to correct plate grip height - # current_pos = self.get_position() - # self.move_joint_angles( - # R=current_pos[0], - # Z=locations[location].joint_angles[1] + grip_height_in_steps, - # P=current_pos[2], - # Y=current_pos[3], - # ) - - - - - - - - # # get joint values of given location - # joint_values = resource_defs.location.joint_angles - # current_pos = self.get_position() # get joint values of current position - - # self.plate_above_height = resource_defs.location.safe_approach_height - # module_safe_height = joint_values[1] + self.plate_above_height - # self.module_safe_height = module_safe_height - - # height_jog_steps = current_pos[1] - module_safe_height # first step of safe approach - - # self.move_joint_angles(R=resource_defs.location.joint_angles[0], Z=current_pos[1], P=current_pos[2], Y=current_pos[3]) - # # self.move_single_axis("R", location) # TODO: adjust to use config instead of values stored in platecrane - # current_pos = self.get_position() # get joint values of current position - # self.move_joint_angles(R=current_pos[0], Z=current_pos[1], P=resource_defs.location.joint_angles[2], Y=current_pos[3]) - # # self.move_single_axis("P", location) - # self.jog("Z", -height_jog_steps) - def pick_plate_safe_approach( self, source: str, @@ -617,8 +456,7 @@ def pick_plate_safe_approach( :return: None """ - # MOVE TO PICK UP PLATE - self.move_joints_neutral() + # open the gripper self.gripper_open() # Rotate base (R axis) toward plate location @@ -648,7 +486,7 @@ def pick_plate_safe_approach( Y=locations[source].joint_angles[3], ) - # lower arm (z axis) to correct plate grip height + # Lower arm (z axis) to correct plate grip height current_pos = self.get_position() self.move_joint_angles( R=current_pos[0], @@ -660,8 +498,7 @@ def pick_plate_safe_approach( # grip the plate self.gripper_close() - # MOVE WITH PLATE SAFELY BACK TO NEUTRAL - # Move arm up to safe approach height + # Move arm with plate back to safe approach height current_pos = self.get_position() self.move_joint_angles( R=current_pos[0], @@ -682,31 +519,79 @@ def pick_plate_safe_approach( # move rest of joints to neutral location self.move_arm_neutral() + def place_plate_safe_approach( + self, + target: str, + grip_height_in_steps: int, + ) -> None: + """Place a module plate onto a module location. - # def place_plate_safe_approach( - # self, target: str, height_offset: int = 0 - # ) -> None: - # """Place a module plate onto a module location. + :param target: Name of the target location. + :type target: str + :param height_jog_steps: Number of jogging steps that will be used to move the Z axis to the plate location + :type height_jog_steps: int + :raises [PlateCraneLocationException]: [Error for None type locations] + :return: None + """ - # :param target: Name of the target location. - # :type target: str - # :param height_jog_steps: Number of jogging steps that will be used to move the Z axis to the plate location - # :type height_jog_steps: int - # :raises [PlateCraneLocationException]: [Error for None type locations] - # :return: None - # """ + # Rotate base (R axis) toward target location + current_pos = self.get_position() + self.move_joint_angles( + R=locations[target].joint_angles[0], + Z=current_pos[1], + P=current_pos[2], + Y=current_pos[3], + ) + + # Lower z axis to safe_approach_z height + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=locations[target].safe_approach_height, + P=current_pos[2], + Y=current_pos[3], + ) + + # extend arm over plate and rotate gripper to correct orientation + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=current_pos[1], + P=locations[target].joint_angles[2], + Y=locations[target].joint_angles[3], + ) - # self.move_joints_neutral() + # lower arm (z axis) to correct plate grip height + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=locations[target].joint_angles[1] + grip_height_in_steps, + P=current_pos[2], + Y=current_pos[3], + ) - # self.move_nest_approach(target) # faces destination, lowers to safe approach height - # current_pos = self.get_position() - # self.move_joint_angles(R=current_pos[0], Z=current_pos[1], P=current_pos[2], Y=resource_defs.source.joint_angless[3]) - # self.move_single_axis("Y", target) - # # self.jog("Z", -(self.plate_pick_steps_module - height_offset)) - # self.jog("Z", (resource_defs.target.joint_angles[1] - self.module_safe_height + height_offset)) #TODO: test, replace height offsetwith plate dimensions - # self.gripper_open() - # self.jog("Z", resource_defs.target.safe_approach_height) + time.sleep(2) + self.gripper_open() + # Back away using safe approach path + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=locations[target].safe_approach_height, + P=current_pos[2], + Y=current_pos[3], + ) + + # retract arm (Y axis) as much as possible (to same Y axis value as Safe location) + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=current_pos[1], + P=current_pos[2], + Y=locations["Safe"].joint_angles[3], + ) + + self.move_tower_neutral() self.move_arm_neutral() def pick_plate_direct( @@ -714,7 +599,6 @@ def pick_plate_direct( source: str, source_type: str, plate_type: str, - # height_offset: int = 0, grip_height_in_steps: int, has_lid: bool, incremental_lift: bool=False @@ -727,23 +611,19 @@ def pick_plate_direct( :return: None """ - print("PICK PLATE DIRECT CALLED") - self.move_joints_neutral() + # Rotate R axis (base rotation) over the plate current_pos = self.get_position() - - # rotate R axis (base rotation) over the plate self.move_joint_angles( R=locations[source].joint_angles[0], Z=current_pos[1], P=current_pos[2], Y=current_pos[3], ) - # self.move_single_axis("R", source) if source_type == "stack": self.gripper_close() - # self.move_location(source) + # TODO: Should we be extending the arm first, then dropping it? # tap arm on top of plate stack to determine stack height self.move_joint_angles( R=locations[source].joint_angles[0], @@ -752,34 +632,22 @@ def pick_plate_direct( Y=locations[source].joint_angles[3], ) - # move up, open gripper, grab plate at correct height + # Move up, open gripper, grab plate at correct height self.jog("Z", 1000) self.gripper_open() - # calculate z travel from grip height with/without lid + # Calculate z travel from grip height with/without lid if has_lid: z_jog_down_from_plate_top = PlateResource.convert_to_steps(plate_definitions[plate_type].plate_height_with_lid) - grip_height_in_steps - # z_jog_down_from_plate_top = plate_definitions[plate_type].plate_height_with_lid - grip_height_in_steps - else: # does not have lid + else: z_jog_down_from_plate_top = PlateResource.convert_to_steps(plate_definitions[plate_type].plate_height) - grip_height_in_steps - # z_jog_down_from_plate_top = plate_definitions[plate_type].plate_height - grip_height_in_steps - # move down to correct z height to grip plate + # Move down to correct z height to grip plate self.jog("Z", -(1000 + z_jog_down_from_plate_top)) - - - #self.jog("Z", -1000 + plate_definitions[plate_type].grip_height + height_offset) # move down to grab height location - - # current_pos = self.get_position() - # self.move_joint_angles( - # R=current_pos[0], - # Z=current_pos - # ) else: # if source_type == nest: self.gripper_open() - # TODO: might need to account for if the plate is a lid here? self.move_joint_angles( R=locations[source].joint_angles[0], Z=locations[source].joint_angles[1] + grip_height_in_steps, @@ -797,14 +665,13 @@ def pick_plate_direct( self.jog("Z", 100) self.move_tower_neutral() - self.move_arm_neutral() - print("PICK PLATE DIRECT FINISHED") def place_plate_direct( self, target: str, + target_type: str, # TODO: use later to slow speed for target_type = "stack" grip_height_in_steps: str, ) -> None: @@ -815,8 +682,6 @@ def place_plate_direct( :return: None """ - self.move_arm_neutral() - # Rotate base (R axis) to target location current_pos = self.get_position() self.move_joint_angles( @@ -898,25 +763,6 @@ def remove_lid( :raises [ErrorType]: [ErrorDescription] :return: None """ - # self.get_new_plate_height(plate_type) # pulls plate dimenstions, now do from config - #TODO: pull plate dimenstions? - # plate_height = resource_defs.plate_type.plate_height - - # target_offset = ( - # 2 * self.plate_above_height - self.plate_pick_steps_stack + self.lid_destination_height - # # + height_offset - # ) # Finding the correct target hight when only transferring the plate lid - # target_loc = self.get_location_joint_values(target) - # remove_lid_target = "Temp_Lid_Target_Loc" - - # self.set_location( - # remove_lid_target, - # target_loc[0], - # target_loc[1] - target_offset, - # target_loc[2], - # target_loc[3], - # ) - # self.plate_pick_steps_stack = self.plate_lid_steps + height_offset source_grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].lid_removal_grip_height + height_offset) target_grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].plate_height_with_lid - plate_definitions[plate_type].lid_height + height_offset) @@ -932,7 +778,6 @@ def remove_lid( incremental_lift = True, ) - def replace_lid( self, source: str, @@ -953,28 +798,9 @@ def replace_lid( :return: None """ - # self.get_new_plate_height(plate_type) - - # target_offset = ( - # 2 * self.plate_above_height - self.plate_pick_steps_stack + self.lid_destination_height - # ) - - # source_loc = self.get_location_joint_values(source) - # remove_lid_source = "Temp_Lid_Source_loc" - - # self.set_location( - # remove_lid_source, - # source_loc[0], - # source_loc[1] - target_offset, - # source_loc[2], - # source_loc[3], - # ) - # self.plate_pick_steps_stack = self.plate_lid_steps + height_offset - source_grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].lid_grip_height + height_offset) target_grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].lid_removal_grip_height + height_offset) - self.transfer( source=source, target=target, @@ -985,46 +811,6 @@ def replace_lid( is_lid=True, ) - - def get_new_plate_height(self, plate_type): - """ - Gets the new plate height values for the given plate_type - :param plate_type: Plate type. - :type source: str - :return: None - """ - if plate_type not in self.plate_resources.keys(): - raise Exception("Unkown plate type") - self.plate_above_height = self.plate_resources[plate_type]["plate_above_height"] - self.plate_lid_steps = self.plate_resources[plate_type]["plate_lid_steps"] - self.plate_pick_steps_stack = self.plate_resources[plate_type][ - "plate_pick_steps_stack" - ] - self.plate_pick_steps_module = self.plate_resources[plate_type][ - "plate_pick_steps_module" - ] - self.lid_destination_height = self.plate_resources[plate_type]["lid_destination_height"] - - def get_stack_resource( - self, - ): - """ - Gets the new plate height values for the given plate_type - :param plate_type: Plate type. - :type source: str - :return: None - """ - pass - - def update_stack_resource(self): - """ - Gets the new plate height values for the given plate_type - :param plate_type: Plate type. - :type source: str - :return: None - """ - pass - def transfer( self, source: str, @@ -1033,83 +819,53 @@ def transfer( height_offset: int = 0, # units = mm is_lid: bool = False, has_lid: bool = False, - source_grip_height_in_steps: int = None, # if iremoving/replacing lid - target_grip_height_in_steps: int = None, # if iremoving/replacing lid + source_grip_height_in_steps: int = None, # if removing/replacing lid + target_grip_height_in_steps: int = None, # if removing/replacing lid incremental_lift: bool = False, - # use_safe_approach: bool = False ) -> None: """ Handles the transfer request - :param source: Source location, provided as either a location name or 4 joint values. + :param source: Source location, provided as a location name from the locations dictionary. :type source: str - :param target: Target location, provided as either a location name or 4 joint values. + :param target: Target location, provided as a location name from the locations dictionary. :type target: str :param plate_type: Type of the plate :type plate_type: str - :raises [ErrorType]: [ErrorDescription] - :return: None + :param height_offset: Height in mm to be applied to default plate grip height. + :type height_offset: int (units = mm) + :param is_lid: True if transferring a lid, False otherwise + :type is_lid: bool + :param has_lid: True if the plate being transferred has a lid, False otherwise + :type has_lid: bool + :param source_grip_height_in_steps: Height at which to grip plate at source (Only used if transfer function is called from remove/replace lid functions) + :type source_grip_height_in_steps: int + :param target_grip_height_in_steps: Height at which to grip plate at target (Only used if transfer function is called from remove/replace lid functions) + :type target_grip_height_in_steps: int + :param incremental_lift: If True will slowly raise after gripping at source (Only used if transfer function is called from remove lid function) + :type incremental_lift: bool + :raises [ErrorType]: [ErrorDescription] # TODO + :return: None # TODO """ - # # self.get_stack_resource() # doesn't do anything - # if plate_type: - # # self.get_new_plate_height(plate_type) # TODO: replace with pulling plate dimenstions from config (or just do later as needed), for now just putting plate type in global - # self.plate_type = plate_type - # # if source_type == "stack" or target_type == "stack": - # # self.stack_transfer(source, target, source_type, target_type, height_offset, incremental_lift) - # # elif source_type == "module" and target_type == "module": - # # self.module_transfer(source, target, height_offset) - # Extract the source and target location_types source_type = locations[source].location_type target_type = locations[target].location_type - - # PICK HEIGHT - # Determine pick height from bottom of plate (converted to z motor steps) - + + # Determine source and target grip heights from bottom of plate (converted from mm to z motor steps) + """If the transfer function is called from either remove_lid() or replace_lid(), + these values will be precalculated and passed in. Otherwise they need to be calculated here.""" if not is_lid: grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].grip_height + height_offset) source_grip_height_in_steps = grip_height_in_steps target_grip_height_in_steps = grip_height_in_steps - # if is_lid: - # grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].lid_grip + height_offset) - - # # calculations - - - # else: # not a lid - # grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].grip_height + height_offset) - - # # Define temporary pick location in plate crane memory #TODO: change this so we're never setting new location in the plate crane - # self.set_location( - # "pick_location_temp", - # locations[source].joint_angles[0], - # locations[source].joint_angles[1] + pick_height_in_steps, - # locations[source].joint_angles[2], - # locations[source].joint_angles[3], - # ) - - # # PLACE HEIGHT - # # Define temporary place location based on pick_height_in_steps variable. - # self.set_location - # is safe approach required for source and/or target? source_use_safe_approach = False if locations[source].safe_approach_height == 0 else True target_use_safe_approach = False if locations[target].safe_approach_height == 0 else True - print(f"Source safe approach: {source_use_safe_approach}") - print(f"Target safe approach: {target_use_safe_approach}") - # Grab from source location + # PICK PLATE FROM SOURCE LOCATION if source_type == "stack": - # self.stack_pick( - # source=source, - # source_type=source_type, - # plate_type=plate_type, - # grip_height_in_steps=grip_height_in_steps, - # has_lid=has_lid, - # incremental_lift=incremental_lift - # ) self.pick_plate_direct( source=source, source_type=source_type, # "stack" @@ -1135,260 +891,36 @@ def transfer( has_lid=has_lid, incremental_lift=incremental_lift, ) - - - # self.nest_pick( - # source=source, - # plate_type=plate_type, - # #height_offset=height_offset, - # grip_height_in_steps=grip_height_in_steps, - # incremental_lift=incremental_lift, - # use_safe_approach=source_use_safe_approach, - # ) else: raise Exception( "Source location type not defined correctly" ) - # PLACE PLATE + # PLACE PLATE AT TARGET LOCATION if target_type == "stack": - self.stack_place( - target=target, - grip_height_in_steps=target_grip_height_in_steps, - ) - elif target_type == "nest": - self.nest_place( + self.place_plate_direct( target=target, target_type=target_type, - plate_type=plate_type, grip_height_in_steps=target_grip_height_in_steps, - use_safe_approach=target_use_safe_approach, - ) + ) + elif target_type == "nest": + if target_use_safe_approach: + self.place_plate_safe_approach( + target=target, + grip_height_in_steps=target_grip_height_in_steps, + ) + else: + self.place_plate_direct( + target=target, + target_type=target_type, + grip_height_in_steps=target_grip_height_in_steps, + ) else: raise Exception( "Target location type not defined correctly" ) - - - self.move_joints_neutral() - self.move_joint_angles( - R=locations["Safe"].joint_angles[0], - Z=locations["Safe"].joint_angles[1], - P=locations["Safe"].joint_angles[2], - Y=locations["Safe"].joint_angles[3] - ) - - # self.update_stack_resource() #TODO: does nothing, could be nice preventing slamming - - def stack_pick( - self, - source: str, - source_type: str, - plate_type: str, - # height_offset: int = 0, - grip_height_in_steps: int, - has_lid: bool, - incremental_lift: bool = False, - ) -> None: - """ - Transfer a plate plate from a plate stack to the exchange location or make a transfer in between stacks and stack entry locations - - :param source: Source location, provided as either a location name or 4 joint values. - :type source: str - :param target: Target location, provided as either a location name or 4 joint values. - :type target: str - :raises [ErrorType]: [ErrorDescription] - :return: None - """ - - # source = self._is_location_joint_values(location=source, name="source") # TODO: removed coordinate functionality for simplicity - - # block here sets source location as new location in platecrane, can remove - # source_loc = self.get_location_joint_values(source) - # stack_source = "stack_source_loc" - # source_offset = self.plate_above_height + height_offset - - # self.set_location( - # stack_source, - # source_loc[0], - # source_loc[1] + source_offset, - # source_loc[2], - # source_loc[3], - # ) - # self.pick_plate_direct(stack_source, "stack", height_offset=height_offset, is_lid=incremental_lift) - self.pick_plate_direct( - source=source, - source_type=source_type, - plate_type=plate_type, - # height_offset=height_offset, - grip_height_in_steps=grip_height_in_steps, - has_lid=has_lid, - incremental_lift=incremental_lift - ) - - - def stack_place( - self, - target: str, - grip_height_in_steps: str, - ) -> None: - """ - Transfer a plate plate from a plate stack to the exchange location or make a transfer in between stacks and stack entry locations - - :param source: Source location, provided as either a location name or 4 joint values. - :type source: str - :param target: Target location, provided as either a location name or 4 joint values. - :type target: str - :raises [ErrorType]: [ErrorDescription] - :return: None - """ - - # TODO: Do we even need this funtion? - self.place_plate_direct( - target=target, - grip_height_in_steps=grip_height_in_steps, - ) - - - # def nest_pick( - # self, - # source: str, - # plate_type: str, - # grip_height_in_steps: int, - # incremental_lift: bool = False, - # use_safe_approach: bool = False - # ) -> None: - # """ - # Transfer a plate in between two modules using source and target locations - - # :param source: Source location, provided as either a location name or 4 joint values. - # :type source: str - # :param target: Target location, provided as either a location name or 4 joint values. - # :type target: str - # :raises [ErrorType]: [ErrorDescription] - # :return: None - # """ - # self.move_joints_neutral() - # #source = self._is_location_joint_values(location=source, name="source") - # if use_safe_approach: - # self.pick_plate_safe_approach( - # source=source, - # plate_type=plate_type, - # grip_height_in_steps=grip_height_in_steps, - # ) - # else: - # self.pick_plate_direct( - # source=source, - # source_type="nest", - # plate_type=plate_type, - # grip_height_in_steps=grip_height_in_steps, - # incremental_lift=incremental_lift, - # ) - - def nest_place( - self, - target: str, - target_type: str, - plate_type: str, - grip_height_in_steps: int, - use_safe_approach: bool = False - ) -> None: - """ - Transfer a plate in between two modules using source and target locations - - :param source: Source location, provided as either a location name or 4 joint values. - :type source: str - :param target: Target location, provided as either a location name or 4 joint values. - :type target: str - :raises [ErrorType]: [ErrorDescription] - :return: None - """ - - # Rotate base (R axix) toward target location - current_pos = self.get_position() - self.move_joint_angles( - R=locations[target].joint_angles[0], - Z=current_pos[1], - P=current_pos[2], - Y=current_pos[3], - ) - - if use_safe_approach: - # Lower z axis to safe_approach_z height - current_pos = self.get_position() - self.move_joint_angles( - R=current_pos[0], - Z=locations[target].safe_approach_height, - P=current_pos[2], - Y=current_pos[3], - ) - - # extend arm over plate and rotate gripper to correct orientation - current_pos = self.get_position() - self.move_joint_angles( - R=current_pos[0], - Z=current_pos[1], - P=locations[target].joint_angles[2], - Y=locations[target].joint_angles[3], - ) - - # lower arm (z axis) to correct plate grip height - current_pos = self.get_position() - self.move_joint_angles( - R=current_pos[0], - Z=locations[target].joint_angles[1] + grip_height_in_steps, - P=current_pos[2], - Y=current_pos[3], - ) - time.sleep(2) - - self.gripper_open() - - # Back away using safe approach path - current_pos = self.get_position() - self.move_joint_angles( - R=current_pos[0], - Z=locations[target].safe_approach_height, - P=current_pos[2], - Y=current_pos[3], - ) - - # retract arm (Y axis) as much as possible (to same Y axis value as Safe location) - current_pos = self.get_position() - self.move_joint_angles( - R=current_pos[0], - Z=current_pos[1], - P=current_pos[2], - Y=locations["Safe"].joint_angles[3], - ) - - else: - # extend arm over plate and rotate gripper to correct orientation - current_pos = self.get_position() - self.move_joint_angles( - R=current_pos[0], - Z=current_pos[1], - P=locations[target].joint_angles[2], - Y=locations[target].joint_angles[3], - ) - - # lower arm (z axis) to correct plate grip height - current_pos = self.get_position() - self.move_joint_angles( - R=current_pos[0], - Z=locations[target].joint_angles[1] + grip_height_in_steps, - P=current_pos[2], - Y=current_pos[3], - ) - - self.gripper_open() - - - self.move_tower_neutral() - self.move_arm_neutral() - if __name__ == "__main__": """ From 2b8dd251c6a0f3293b2c1e8d7f5b2d632e0851a4 Mon Sep 17 00:00:00 2001 From: Casey Stone Date: Tue, 18 Jun 2024 11:02:51 -0500 Subject: [PATCH 09/19] started editing docstrings --- src/platecrane_driver/platecrane_driver.py | 72 ++++------------------ 1 file changed, 12 insertions(+), 60 deletions(-) diff --git a/src/platecrane_driver/platecrane_driver.py b/src/platecrane_driver/platecrane_driver.py index b8cdd87..d07d61a 100644 --- a/src/platecrane_driver/platecrane_driver.py +++ b/src/platecrane_driver/platecrane_driver.py @@ -18,6 +18,13 @@ # fine tune (z height) of all positions # edit all the doc strings to match new functions # should we be using error_codes.py to be doing some of the error checking/raising +# Fix two references to dev/ttyUSB number. is it 2 or 4? + +# Crash error outputs 21(R axis),14(z axis), 02 Wrong location name. 1400 (Z axis hits the plate), 00 success +# TODO: Need a response handler function. Unkown error messages T1, ATS, TU these are about connection issues (multiple access?) +# TODO: Slow the arm before hitting the plate in pick_stack_plate +# TODO: Create a plate detect function within pick stack plate function +# TODO: Maybe write another pick stack funtion to remove the plate detect movement class PlateCrane: @@ -76,8 +83,8 @@ def initialize(self): def home(self, timeout=28): """Homes all of the axes. Returns to neutral position (above exchange) - :param timeout: [ParamDescription], defaults to [DefaultParamVal] - :type [ParamName]: [ParamType](, optional) + :param timeout: Seconds to wait for plate crane response after sending serial command, defaults to 28 seconds. + :type timeout: int, optional ... :raises [ErrorType]: [ErrorDescription] ... @@ -845,7 +852,7 @@ def transfer( :param incremental_lift: If True will slowly raise after gripping at source (Only used if transfer function is called from remove lid function) :type incremental_lift: bool :raises [ErrorType]: [ErrorDescription] # TODO - :return: None # TODO + :return: None """ # Extract the source and target location_types @@ -929,60 +936,5 @@ def transfer( s = PlateCrane("/dev/ttyUSB4") # s.initialize() # s.home() - stack4 = "Stack4" - stack5 = "Stack5" - solo6 = "Solo.Position6" - solo4 = "Solo.Position4" - solo3 = "Solo.Position3" - target_loc = "HidexNest2" - lidnest3 = "LidNest3" - sealer = "SealerNest" - # s.move_location("Safe") - -# TESTING -# s.pick_stack_plate("Stack1") -# a = s.get_position() -# s.set_location("LidNest3",R=231449,Z=-31500,P=484,Y=-306) - -# s.set_location("Hidex.Nest",R=a[0],Z=a[1],P=a[2],Y=a[3]) -# s.place_module_plate("Hidex.Nest") -# s.move_single_axis("Z","Hidex.Nest") -# s.transfer("Hidex.Nest","Solo.Position1",source_type="module",target_type="stack",height_offset=800) -# s.transfer("Stack1", "PeelerNest",source_type="stack",target_type="stack") - -# s.place_module_plate() -# s.get_location_list() - -# s.move_joints_neutral() -# s.move_single_axis("R", "Safe", delay_time=1) -# s.set_location("Safe",R=195399,Z=0,P=0,Y=0) -# s.set_location("LidNest2",R=131719,Z=-31001,P=-5890,Y=-315) -# s.transfer(source="LidNest1",target="LidNest2",source_type="stack",target_type="stack", plate_type="96_well") - -# s.transfer(source="LidNest2",target="LidNest3",source_type="stack",target_type="stack", plate_type="96_well") -# s.transfer("Stack1","Stack1") -# s.free_joints() -# s.lock_joints() - -# s.set_location("LidNest3",R=99817,Z=-31001,P=-5890,Y=-315) - -# s.get_location_joint_values("HidexNest2") -# s.set_location("HidexNest2", R=210015,Z=-30400,P=490,Y=2323) - -# s.transfer(stack5, solo4, source_type = "stack", target_type = "module", plate_type = "96_deep_well") -# s.transfer(solo4, stack5, source_type = "module", target_type = "stack", plate_type = "96_deep_well") - -# s.remove_lid(source = "LidNest1", target="LidNest2", plate_type="96_well") -# s.transfer("Stack4", solo3, source_type = "stack", target_type = "stack", plate_type = "tip_box_lid_off") -# s.remove_lid(source = solo6, target="LidNest3", plate_type="tip_box_lid_on") -# s.replace_lid(source = "LidNest3", target = solo6, plate_type = "tip_box_lid_on") -# s.replace_lid(source = "LidNest2", target = solo4, plate_type = "96_well") -# s.transfer(solo4, stack5, source_type = "module", target_type = "stack", plate_type = "96_well") -# s.transfer(solo6, "Stack2", source_type = "module", target_type = "stack", plate_type = "tip_box_lid_on") - - -# Crash error outputs 21(R axis),14(z axis), 02 Wrong location name. 1400 (Z axis hits the plate), 00 success -# TODO: Need a response handler function. Unkown error messages T1, ATS, TU these are about connection issues (multiple access?) -# TODO: Slow the arm before hitting the plate in pick_stack_plate -# TODO: Create a plate detect function within pick stack plate function -# TODO: Maybe write another pick stack funtion to remove the plate detect movement + + From 396471d8d6fa3b11780bcb93ef2b89aa3fefcb8b Mon Sep 17 00:00:00 2001 From: caseystone Date: Tue, 18 Jun 2024 14:17:09 -0500 Subject: [PATCH 10/19] all locations working, added timeout to move_joints --- src/platecrane_driver/platecrane_driver.py | 29 ++++++++++++---------- src/platecrane_driver/resource_defs.py | 26 +++++++++---------- 2 files changed, 29 insertions(+), 26 deletions(-) diff --git a/src/platecrane_driver/platecrane_driver.py b/src/platecrane_driver/platecrane_driver.py index d07d61a..f1078d9 100644 --- a/src/platecrane_driver/platecrane_driver.py +++ b/src/platecrane_driver/platecrane_driver.py @@ -4,13 +4,13 @@ from pathlib import Path import time -from platecrane_driver.serial_port import SerialPort # use when running through WEI REST clients -from platecrane_driver.resource_defs import locations, plate_definitions -from platecrane_driver.resource_types import PlateResource +# from platecrane_driver.serial_port import SerialPort # use when running through WEI REST clients +# from platecrane_driver.resource_defs import locations, plate_definitions +# from platecrane_driver.resource_types import PlateResource -# from serial_port import SerialPort # use when running through the driver -# from resource_defs import locations, plate_definitions -# from resource_types import PlateResource +from serial_port import SerialPort # use when running through the driver +from resource_defs import locations, plate_definitions +from resource_types import PlateResource # TODOs: @@ -173,11 +173,11 @@ def get_position(self) -> list: """ Requests and stores plate_crane position. Coordinates: - Z: Vertical axis R: Base turning axis - Y: Extension axis + Z: Vertical axis P: Gripper turning axis - + Y: Extension axis + :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] :type [ParamName]: [ParamType](, optional) ... @@ -193,12 +193,15 @@ def get_position(self) -> list: try: # collect coordinates of current position - current_position = list(self.__serial_port.send_command(command).split(" ")) + # time.sleep(2) + current_position = list(self.__serial_port.send_command(command,1).split(" ")) + print(current_position) current_position = [eval(x.strip(",")) for x in current_position] + print(current_position) except Exception: print("Overlapping serial responses detected. Waiting 5 seconds to resend latest command") time.sleep(5) - current_position = list(self.__serial_port.send_command(command).split(" ")) + current_position = list(self.__serial_port.send_command(command,1).split(" ")) current_position = [eval(x.strip(",")) for x in current_position] return current_position @@ -342,7 +345,7 @@ def move_joint_angles(self, R: int, Z: int, P: int, Y: int) -> None: command = "MOVE TEMP\r\n" try: - self.__serial_port.send_command(command) + self.__serial_port.send_command(command, 2) except Exception as err: print(err) @@ -654,7 +657,7 @@ def pick_plate_direct( else: # if source_type == nest: self.gripper_open() - + self.move_joint_angles( R=locations[source].joint_angles[0], Z=locations[source].joint_angles[1] + grip_height_in_steps, diff --git a/src/platecrane_driver/resource_defs.py b/src/platecrane_driver/resource_defs.py index 9306015..8037b33 100644 --- a/src/platecrane_driver/resource_defs.py +++ b/src/platecrane_driver/resource_defs.py @@ -1,26 +1,25 @@ -#from resource_types import Location, PlateResource -from platecrane_driver.resource_types import Location, PlateResource # through WEI +from resource_types import Location, PlateResource +# from platecrane_driver.resource_types import Location, PlateResource # through WEI # Locations accessible by the PlateCrane EX locations = { "Safe": Location(name="Safe", joint_angles=[182220, 2500, 460, -308], location_type="nest", safe_approach_height=0), - "Stack1": Location(name="Stack1", joint_angles=[164672, -32623, 472, 5389], location_type="stack", safe_approach_height=0), - "Stack2": Location(name="Stack2", joint_angles=[182220, -32623, 460, 5420], location_type="stack", safe_approach_height=0), - "Stack3": Location(name="Stack3", joint_angles=[199708, -32623, 514, 5484], location_type="stack", safe_approach_height=0), - "Stack4": Location(name="Stack4", joint_angles=[217401, -32623, 546, 5473], location_type="stack", safe_approach_height=0), - "Stack5": Location(name="Stack5", joint_angles=[235104, -32623, 532, 5453], location_type="stack", safe_approach_height=0), - "LidNest1": Location(name="LidNest1", joint_angles=[168355, -31500, 484, -306], location_type="nest", safe_approach_height=0), - "LidNest2": Location(name="LidNest2", joint_angles=[199805, -31500, 484, -306], location_type="nest", safe_approach_height=0), - "LidNest3": Location(name="LidNest3", joint_angles=[231449, -31500, 484, -306], location_type="nest", safe_approach_height=0), + "Stack1": Location(name="Stack1", joint_angles=[164672, -32703, 472, 5389], location_type="stack", safe_approach_height=0), + "Stack2": Location(name="Stack2", joint_angles=[182220, -32703, 460, 5420], location_type="stack", safe_approach_height=0), + "Stack3": Location(name="Stack3", joint_angles=[199708, -32703, 514, 5484], location_type="stack", safe_approach_height=0), + "Stack4": Location(name="Stack4", joint_angles=[217401, -32703, 546, 5473], location_type="stack", safe_approach_height=0), + "Stack5": Location(name="Stack5", joint_angles=[235104, -32703, 532, 5453], location_type="stack", safe_approach_height=0), + "LidNest1": Location(name="LidNest1", joint_angles=[168355, -31800, 484, -306], location_type="nest", safe_approach_height=0), + "LidNest2": Location(name="LidNest2", joint_angles=[199805, -31800, 484, -306], location_type="nest", safe_approach_height=0), + "LidNest3": Location(name="LidNest3", joint_angles=[231449, -31800, 484, -306], location_type="nest", safe_approach_height=0), "Solo.Position1": Location(name="Solo.Position1", joint_angles=[36703, -27951, -1000, 3630], location_type="nest", safe_approach_height=0), "Solo.Position2": Location(name="Solo.Position2", joint_angles=[53182, -27951, -413, 834], location_type="nest", safe_approach_height=0), "Hidex.Nest": Location(name="Hidex.Nest", joint_angles=[102327, -31090, -5830, 2378], location_type="nest", safe_approach_height=-27033), - "Sealer.Nest": Location(name="Sealer.Nest", joint_angles=[117468, 1220, -4748, 4550], location_type="nest", safe_approach_height=0), - "Peeler.Nest": Location(name="Peeler.Nest", joint_angles=[292611, -30758, -4469, 4257], location_type="nest", safe_approach_height=0), + "Sealer.Nest": Location(name="Sealer.Nest", joint_angles=[117305, 920, -4766, 4550], location_type="nest", safe_approach_height=0), + "Peeler.Nest": Location(name="Peeler.Nest", joint_angles=[292635, -31058, -4521, 4235], location_type="nest", safe_approach_height=0), "Liconic.Nest": Location(name="Liconic.Nest", joint_angles=[265603, -19900, -5413, 4953], location_type="nest", safe_approach_height=0), } -#19831 # Dimensions of labware used on the BIO_Workcells plate_definitions = { "flat_bottom_96well": PlateResource(plate_height = 14, grip_height=3, plate_height_with_lid=16, lid_height=10, lid_grip_height=4, lid_removal_grip_height=12), @@ -28,3 +27,4 @@ "pcr_96well": PlateResource(plate_height = 0, grip_height=0, plate_height_with_lid=0, lid_height=0, lid_grip_height=0, lid_removal_grip_height=0), } +# R, Z, P, Y From 6a20849a9acdd3c973e2ccfcb76a150108603502 Mon Sep 17 00:00:00 2001 From: Casey Stone Date: Thu, 20 Jun 2024 15:17:53 -0500 Subject: [PATCH 11/19] cleanup of driver and resource_defs --- src/platecrane_driver/platecrane_driver.py | 537 ++++++++++----------- src/platecrane_driver/resource_defs.py | 8 +- 2 files changed, 247 insertions(+), 298 deletions(-) diff --git a/src/platecrane_driver/platecrane_driver.py b/src/platecrane_driver/platecrane_driver.py index f1078d9..db0b61f 100644 --- a/src/platecrane_driver/platecrane_driver.py +++ b/src/platecrane_driver/platecrane_driver.py @@ -4,77 +4,56 @@ from pathlib import Path import time -# from platecrane_driver.serial_port import SerialPort # use when running through WEI REST clients -# from platecrane_driver.resource_defs import locations, plate_definitions -# from platecrane_driver.resource_types import PlateResource - -from serial_port import SerialPort # use when running through the driver -from resource_defs import locations, plate_definitions -from resource_types import PlateResource +from platecrane_driver.serial_port import SerialPort # use when running through WEI REST clients +from platecrane_driver.resource_defs import locations, plate_definitions +from platecrane_driver.resource_types import PlateResource +# from serial_port import SerialPort # use when running through the driver +# from resource_defs import locations, plate_definitions +# from resource_types import PlateResource +""" # TODOs: -# look into how to slow speed of stack pick and place -# fine tune (z height) of all positions -# edit all the doc strings to match new functions -# should we be using error_codes.py to be doing some of the error checking/raising -# Fix two references to dev/ttyUSB number. is it 2 or 4? - -# Crash error outputs 21(R axis),14(z axis), 02 Wrong location name. 1400 (Z axis hits the plate), 00 success -# TODO: Need a response handler function. Unkown error messages T1, ATS, TU these are about connection issues (multiple access?) -# TODO: Slow the arm before hitting the plate in pick_stack_plate -# TODO: Create a plate detect function within pick stack plate function -# TODO: Maybe write another pick stack funtion to remove the plate detect movement + * combine two initialization functions + * Look into how to slow speed of stack pick and place + * should we be using error_codes.py to be doing some of the error checking/raising + * Crash error outputs 21(R axis),14(z axis), 02 Wrong location name. 1400 (Z axis hits the plate), 00 success + * Need a response handler function. Unknown error messages T1, ATS, TU these are about connection issues (multiple access?) + * Maybe create a plate detect function within pick stack plate function +""" class PlateCrane: - """ - Description: - Python interface that allows remote commands to be executed to the plate_crane. - """ + """ Python interface that allows remote commands to be executed to the plate_crane.""" __serial_port: SerialPort - def __init__(self, host_path="/dev/ttyUSB2", baud_rate=9600): - """[Summary] + def __init__(self, host_path="/dev/ttyUSB4", baud_rate=9600): + """Initialization function - :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] - :type [ParamName]: [ParamType](, optional) - ... - :raises [ErrorType]: [ErrorDescription] - ... - :return: [ReturnDescription] - :rtype: [ReturnType] + Args: + host_path (str): usb path of PlateCrane EX device + baud_rate (int): baud rate to use for communication with the PlateCrane EX device + + Returns: + None """ + # define variables self.__serial_port = SerialPort(host_path=host_path, baud_rate=baud_rate) self.robot_error = "NO ERROR" self.status = 0 self.error = "" - self.robot_status = "" self.movement_state = "READY" self.platecrane_current_position = None - # self.stack_resources = json.load( - # open(Path(__file__).parent / "stack_resources.json") - # ) - + # initialize actions self.initialize() def initialize(self): - """[Summary] - - :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] - :type [ParamName]: [ParamType](, optional) - ... - :raises [ErrorType]: [ErrorDescription] - ... - :return: [ReturnDescription] - :rtype: [ReturnType] - """ - + """Initialization actions""" self.get_status() if self.robot_status == "0": self.home() @@ -83,32 +62,19 @@ def initialize(self): def home(self, timeout=28): """Homes all of the axes. Returns to neutral position (above exchange) - :param timeout: Seconds to wait for plate crane response after sending serial command, defaults to 28 seconds. - :type timeout: int, optional - ... - :raises [ErrorType]: [ErrorDescription] - ... - :return: [ReturnDescription] - :rtype: [ReturnType] + Args: + timeout (int): Seconds to wait for plate crane response after sending serial command, defaults to 28 seconds. + + Returns: + None """ # Moves axes to home position command = "HOME\r\n" self.__serial_port.send_command(command, timeout) - def get_status(self): - """Checks status of plate_crane - - :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] - :type [ParamName]: [ParamType](, optional) - ... - :raises [ErrorType]: [ErrorDescription] - ... - :return: [ReturnDescription] - :rtype: [ReturnType] - """ - + """Checks status of plate_crane""" command = "STATUS\r\n" self.robot_status = self.__serial_port.send_command(command) @@ -123,16 +89,7 @@ def lock_joints(self): self.__serial_port.send_command(command) def get_location_list(self): - """Checks status of plate_crane - - :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] - :type [ParamName]: [ParamType](, optional) - ... - :raises [ErrorType]: [ErrorDescription] - ... - :return: [ReturnDescription] - :rtype: [ReturnType] - """ + """Displays all location information stored in the Plate Crane EX robot's memory""" command = "LISTPOINTS\r\n" out_msg = self.__serial_port.send_command(command) @@ -151,15 +108,23 @@ def get_location_list(self): def get_location_joint_values(self, location: str = None) -> list: - """Checks status of plate_crane - - :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] - :type [ParamName]: [ParamType](, optional) - ... - :raises [ErrorType]: [ErrorDescription] - ... - :return: [ReturnDescription] - :rtype: [ReturnType] + """Returns list of 4 joint values associated with a position name + + Note: right now this returns the joint values stored in the + PlateCrane EX device memory, not the locations stored in + resource_defs.py + + TODO: delete this function or associate it with resource_defs.py + + Args: + location (str): Name of location + + Returns: + joint_values ([int]): [R, Z, P, Y] joint values + - R (base rotation) + - Z (arm vertical axis) + - P (gripper rotation) + - Y (arm extension) """ command = "GETPOINT " + location + "\r\n" @@ -170,36 +135,29 @@ def get_location_joint_values(self, location: str = None) -> list: return joint_values def get_position(self) -> list: - """ - Requests and stores plate_crane position. - Coordinates: - R: Base turning axis - Z: Vertical axis - P: Gripper turning axis - Y: Extension axis + """Returns list of joint values for current position of the PlateCrane EX arm + + Args: + None - :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] - :type [ParamName]: [ParamType](, optional) - ... - :raises [ErrorType]: [ErrorDescription] - ... - :return: [ReturnDescription] - :rtype: [ReturnType] + Returns: + current_position ([int]): [R, Z, P, Y] joint values + - R (base rotation) + - Z (arm vertical axis) + - P (gripper rotation) + - Y (arm extension) """ command = "GETPOS\r\n" - # time.sleep(2) # helps to reduce serial port overlapping responses - try: # collect coordinates of current position - # time.sleep(2) current_position = list(self.__serial_port.send_command(command,1).split(" ")) print(current_position) current_position = [eval(x.strip(",")) for x in current_position] print(current_position) except Exception: - print("Overlapping serial responses detected. Waiting 5 seconds to resend latest command") + # Fall back: overlapping serial responses were detected. Wait 5 seconds then resend latest command time.sleep(5) current_position = list(self.__serial_port.send_command(command,1).split(" ")) current_position = [eval(x.strip(",")) for x in current_position] @@ -214,15 +172,17 @@ def set_location( P: int = 0, Y: int = 0, ): - """Saves a new location onto robot - - :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] - :type [ParamName]: [ParamType](, optional) - ... - :raises [ErrorType]: [ErrorDescription] - ... - :return: [ReturnDescription] - :rtype: [ReturnType] + """Saves a new location into PlateCrane EX device memory + + Args: + location_name (str): Name of location to be saved + R (int): base rotation (units: motor steps) + Z (int): vertical axis (units: motor steps) + P (int): gripper rotation (units: motor steps) + Y (int): arm extension (units: motor steps) + + Returns: + None """ command = "LOADPOINT %s, %s, %s, %s, %s\r\n" % ( @@ -235,15 +195,13 @@ def set_location( self.__serial_port.send_command(command) def delete_location(self, location_name: str = None): - """Deletes a location from the robot's database - - :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] - :type [ParamName]: [ParamType](, optional) - ... - :raises [ErrorType]: [ErrorDescription] - ... - :return: [ReturnDescription] - :rtype: [ReturnType] + """Deletes an existing location from the PlateCrane EX device memory + + Args: + location_name (str): Name of location to delete + + Returns: + None """ if not location_name: raise Exception("No location name provided") @@ -254,61 +212,25 @@ def delete_location(self, location_name: str = None): self.__serial_port.send_command(command) def gripper_open(self): - """Opens gripper - - :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] - :type [ParamName]: [ParamType](, optional) - ... - :raises [ErrorType]: [ErrorDescription] - ... - :return: [ReturnDescription] - :rtype: [ReturnType] - """ + """Opens gripper""" command = "OPEN\r\n" self.__serial_port.send_command(command) def gripper_close(self): - """Closes gripper - - :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] - :type [ParamName]: [ParamType](, optional) - ... - :raises [ErrorType]: [ErrorDescription] - ... - :return: [ReturnDescription] - :rtype: [ReturnType] - """ + """Closes gripper""" command = "CLOSE\r\n" self.__serial_port.send_command(command) def check_open(self): - """Checks if gripper is open - - :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] - :type [ParamName]: [ParamType](, optional) - ... - :raises [ErrorType]: [ErrorDescription] - ... - :return: [ReturnDescription] - :rtype: [ReturnType] - """ + """Checks if gripper is open""" command = "GETGRIPPERISOPEN\r\n" self.__serial_port.send_command(command) def check_closed(self): - """Checks if gripper is closed - - :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] - :type [ParamName]: [ParamType](, optional) - ... - :raises [ErrorType]: [ErrorDescription] - ... - :return: [ReturnDescription] - :rtype: [ReturnType] - """ + """Checks if gripper is closed""" command = "GETGRIPPERISCLOSED\r\n" self.__serial_port.send_command(command) @@ -316,32 +238,28 @@ def check_closed(self): def jog(self, axis, distance) -> None: """Moves the specified axis the specified distance. - :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] - :type [ParamName]: [ParamType](, optional) - ... - :raises [ErrorType]: [ErrorDescription] - ... - :return: [ReturnDescription] - :rtype: [ReturnType] + Args: + axis (str): "R", "Z", "P", or "Y" + distance (int): distance to move along the axis (units = motor steps) + + Returns: + None """ command = "JOG %s,%d\r\n" % (axis, distance) self.__serial_port.send_command(command, timeout=1.5) def move_joint_angles(self, R: int, Z: int, P: int, Y: int) -> None: - """Moves on a single axis, using an existing location on robot's database - - :param [ParamName]: [ParamDescription], defaults to [DefaultParamVal] - :type [ParamName]: [ParamType](, optional) - ... - :raises [ErrorType]: [ErrorDescription] - ... - :return: [ReturnDescription] - :rtype: [ReturnType] + """Move to a specified location + + Args: + R (int): base rotation (unit = motor steps) + Z (int): vertical axis (unit = motor steps) + P (int): gripper rotation (unit = motor steps) + Y (int): arm extension (unit = motor steps) """ self.set_location("TEMP", R, Z, P, Y) - command = "MOVE TEMP\r\n" try: @@ -357,18 +275,25 @@ def move_joint_angles(self, R: int, Z: int, P: int, Y: int) -> None: self.delete_location("TEMP") def move_single_axis(self, axis: str, loc: str, delay_time=1.5) -> None: - """Moves on a single axis using an existing location on robot's database + """Moves on a single axis, using an existing location in PlateCrane EX device memory as reference - :param axis: Axis name (R,Z,P,Y) - :type axis: str - :param loc: Name of the location. - :type loc: str + Args: + axis (str): axis to move along + loc (str): name of location in PlateCrane EX device memory to use as reference + delay_time (float): serial command timeout. Seconds to wait before expecting a serial response + defaults to 1.5 seconds - :raises [PlateCraneLocationException]: [Error for None type locations] - :return: None + Raises: + TODO + + Returns: + None + + TODO: + * Handle errors using error_codes.py + * Reference locations in resource_defs.py, not device memory """ - # TODO:Handle the error raising within error_codes.py if not loc: raise Exception( "PlateCraneLocationException: NoneType variable is not compatible as a location" @@ -381,15 +306,17 @@ def move_single_axis(self, axis: str, loc: str, delay_time=1.5) -> None: def move_location(self, loc: str = None, move_time: float = 4.7) -> None: """Moves all joint to the given location. - :param loc: Name of the location. - :type loc: str - :param move_time: Number of seconds that will take to complete this movement. Defaults to 4.7 seconds which is the longest possible movement time. - :type move_time: float - :raises [PlateCraneLocationException]: [Error for None type locations] - :return: None - """ + Args: + loc (str): location to move to + move_time (float): serial command timeout. Seconds to wait before expecting a serial response + defaults to 4.7 seconds - # TODO:Handle the error raising within error_codes.py + Returns: + None + + TODO: + * Handle the error raising within error_codes.py + """ if not loc: raise Exception( "PlateCraneLocationException: NoneType variable is not compatible as a location" @@ -401,10 +328,11 @@ def move_location(self, loc: str = None, move_time: float = 4.7) -> None: def move_tower_neutral(self) -> None: """Moves the tower to neutral position - :return: None + TODO: + * This still creates a TEMP position, moves to it, then deletes it after. + Change this, and other related methods below, to use only access + locations in resource_defs """ - - # TODO: This still creates a TEMP position, moves to it, then deletes it after current_pos = self.get_position() self.move_joint_angles( R=current_pos[0], @@ -414,10 +342,8 @@ def move_tower_neutral(self) -> None: ) def move_arm_neutral(self) -> None: - """Moves the arm to neutral position + """Moves the arm to neutral position""" - :return: None - """ current_pos = self.get_position() self.move_joint_angles( R=current_pos[0], @@ -427,11 +353,8 @@ def move_arm_neutral(self) -> None: ) def move_gripper_neutral(self) -> None: - """Moves the gripper to neutral position - - :return: None - """ - # TODO: this still accesses plate crane intenal safe + """Moves the gripper to neutral position""" + self.move_single_axis("P", "Safe", delay_time=0.3) current_pos = self.get_position() @@ -443,10 +366,7 @@ def move_gripper_neutral(self) -> None: ) def move_joints_neutral(self) -> None: - """Moves all joints neutral position - - :return: None - """ + """Moves all joints neutral position""" self.move_arm_neutral() self.move_tower_neutral() @@ -456,14 +376,15 @@ def pick_plate_safe_approach( plate_type: str, grip_height_in_steps: int, ) -> None: - """Pick a module plate from a module location. - - :param source: Name of the source location. - :type source: str - :param height_jog_steps: Number of jogging steps that will be used to move the Z axis to the plate location - :type height_jog_steps: int - :raises [PlateCraneLocationException]: [Error for None type locations] - :return: None + """Picks a plate from a source type "nest" using a safe travel path. + + Args: + source (str): source location name defined in resource_defs.py + plate_type (str): plate definition name defined in resource_defs.py + grip_height_in_steps (int): z axis steps distance from bottom of plate to grip the plate + + Returns: + None """ # open the gripper @@ -534,14 +455,15 @@ def place_plate_safe_approach( target: str, grip_height_in_steps: int, ) -> None: - """Place a module plate onto a module location. - - :param target: Name of the target location. - :type target: str - :param height_jog_steps: Number of jogging steps that will be used to move the Z axis to the plate location - :type height_jog_steps: int - :raises [PlateCraneLocationException]: [Error for None type locations] - :return: None + """Places a plate to a target location of type "nest" using a safe travel path. + + Args: + target (str): source location name defined in resource_defs.py + plate_type (str): plate definition name defined in resource_defs.py + grip_height_in_steps (int): z axis steps distance from bottom of plate to grip the plate + + Returns: + None """ # Rotate base (R axis) toward target location @@ -601,6 +523,7 @@ def place_plate_safe_approach( Y=locations["Safe"].joint_angles[3], ) + # move arm to safe location self.move_tower_neutral() self.move_arm_neutral() @@ -613,12 +536,25 @@ def pick_plate_direct( has_lid: bool, incremental_lift: bool=False ) -> None: - """Pick a stack plate from stack location. + """Picks a plate from a source location of type either "nest" or "stack" using a direct travel path + + "nest" transfers: gripper open, direct to grab plate z height + "stack" transfers: gripper closed, touch top of plate, z up, z down to correct grab plate height - :param source: Name of the source location. - :type source: str - :raises [PlateCraneLocationException]: [Error for None type locations] - :return: None + Args: + source (str): source location name defined in resource_defs.py + source_type (str): either "nest" or "stack" + plate_type (str): plate definition name defined in resource_defs.py + grip_height_in_steps (int): z axis steps distance from bottom of plate to grip the plate + has_lid (bool): True if plate has lid, False otherwise + incremental_lift (bool): True if you want to use incremental lift, False otherwise (default False) + incremental lift (good for ensuring lids are removed gently and correctly): + - grab plate at grip_height_in_steps + - raise 100 steps along z axis (repeat 5x) + - continue with rest of transfer + + Returns: + None """ # Rotate R axis (base rotation) over the plate @@ -631,9 +567,9 @@ def pick_plate_direct( ) if source_type == "stack": + # close the gripper self.gripper_close() - # TODO: Should we be extending the arm first, then dropping it? # tap arm on top of plate stack to determine stack height self.move_joint_angles( R=locations[source].joint_angles[0], @@ -664,7 +600,8 @@ def pick_plate_direct( P=locations[source].joint_angles[2], Y=locations[source].joint_angles[3], ) - + + # open the gripper self.gripper_close() if incremental_lift: @@ -674,6 +611,7 @@ def pick_plate_direct( self.jog("Z", 100) self.jog("Z", 100) + # return arm to safe location self.move_tower_neutral() self.move_arm_neutral() @@ -684,12 +622,19 @@ def place_plate_direct( target_type: str, # TODO: use later to slow speed for target_type = "stack" grip_height_in_steps: str, ) -> None: + """Places a plate onto a target location of type either "nest" or "stack" using a direct travel path + + Args: + target (str): target location name defined in resource_defs.py + target_type (str): either "nest" or "stack" + plate_type (str): plate definition name defined in resource_defs.py + grip_height_in_steps (int): z axis steps distance from bottom of plate to grip the plate - """Place a stack plate either onto the exhange location or into a stack + Returns: + None - :param target: Name of the target location. Defults to None if target is None, it will be set to exchange location. - :type target: str - :return: None + TODO: + * use target_type variable to slow approach in "stack" transfers to avoid striking other plates """ # Rotate base (R axis) to target location @@ -726,16 +671,12 @@ def place_plate_direct( def _is_location_joint_values(self, location: str, name: str = "temp") -> str: """ - If the location was provided as joint values, transfer joint values into a saved location on the robot and return the location name. - If location parameter is a name of an already saved location, do nothing. - - :param location: Location to be checked if this is an already saved location on the robot database or a new location with 4 joint values - :type location: string - :param name: Location name to be used to save a new location if the location parameter was provided as 4 joint values - :type name: string - :raises [ErrorType]: [ErrorDescription] - :return: location_name = Returns the location name that is saved on robot database with location joint values - :rtype: str + If the location was provided as joint values, transfer joint values into a saved location + on the robot and return the location name. If location parameter is a name of an already saved + location, do nothing. + + TODO: + * Is there any reason we should keep this function? """ try: # location = eval(location) # replacing with checking config @@ -761,22 +702,24 @@ def remove_lid( plate_type: str, height_offset: int = 0, ) -> None: - """ - Remove the plate lid - - :param source: Source location, provided as either a location name or 4 joint values. - :type source: str - :param target: Target location, provided as either a location name or 4 joint values. - :type target: str - :param plate_type: Type of the plate - :type plate_type: str - :raises [ErrorType]: [ErrorDescription] - :return: None + """Removes lid from a plate at source location and places lid at target location + + Args: + source (str): source location name defined in resource_defs.py + target (str): target location name defined in resource_defs.py + plate_type (str): plate definition name defined in resource_defs.py + height_offset (int): change in z height to be applied to grip location on the lid (units = mm) + defaults to 0mm + + Returns: + None """ + # Calculate grip height in motor steps source_grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].lid_removal_grip_height + height_offset) target_grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].plate_height_with_lid - plate_definitions[plate_type].lid_height + height_offset) + # Pass to transfer function but specify that it is a lid we're transferring self.transfer( source=source, target=target, @@ -795,22 +738,23 @@ def replace_lid( plate_type: str, height_offset: int = 0, ) -> None: - """ - Replace the lid back to the plate - - :param source: Source location, provided as either a location name or 4 joint values. - :type source: str - :param target: Target location, provided as either a location name or 4 joint values. - :type target: str - :param plate_type: Type of the plate - :type plate_type: str - :raises [ErrorType]: [ErrorDescription] - :return: None - """ + """"Replaces lid at source location onto a plate at the target location + Args: + source (str): source location name defined in resource_defs.py + target (str): target location name defined in resource_defs.py + plate_type (str): plate definition name defined in resource_defs.py + height_offset (int): change in z height to be applied to grip location on the lid (units = mm) + defaults to 0mm + + Returns: + None + """ + # Calculate grip height in motor steps source_grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].lid_grip_height + height_offset) target_grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].lid_removal_grip_height + height_offset) + # Pass to transfer function but specify that it is a lid we're transferring self.transfer( source=source, target=target, @@ -833,29 +777,35 @@ def transfer( target_grip_height_in_steps: int = None, # if removing/replacing lid incremental_lift: bool = False, ) -> None: - """ - Handles the transfer request - - :param source: Source location, provided as a location name from the locations dictionary. - :type source: str - :param target: Target location, provided as a location name from the locations dictionary. - :type target: str - :param plate_type: Type of the plate - :type plate_type: str - :param height_offset: Height in mm to be applied to default plate grip height. - :type height_offset: int (units = mm) - :param is_lid: True if transferring a lid, False otherwise - :type is_lid: bool - :param has_lid: True if the plate being transferred has a lid, False otherwise - :type has_lid: bool - :param source_grip_height_in_steps: Height at which to grip plate at source (Only used if transfer function is called from remove/replace lid functions) - :type source_grip_height_in_steps: int - :param target_grip_height_in_steps: Height at which to grip plate at target (Only used if transfer function is called from remove/replace lid functions) - :type target_grip_height_in_steps: int - :param incremental_lift: If True will slowly raise after gripping at source (Only used if transfer function is called from remove lid function) - :type incremental_lift: bool - :raises [ErrorType]: [ErrorDescription] # TODO - :return: None + """Handles the transfer request + + Args: + source (str): source location name defined in resource_defs.py + target (str): target location name defined in resource_defs.py + plate_type (str): plate definition name defined in resource_defs.py + height_offset (int): change in z height to be applied to grip location on the lid (units = mm) + defaults to 0mm + is_lid (bool): True if transferring a lid, False otherwise + defaults to False + has_lid (bool): True if plate being transferred has a lid, otherwise False + defaults to False + source_grip_height_in_steps (int): z axis steps distance from bottom of plate to grip the plate at source location + defaults to None + only used if transfer function is called from remove/replace_lid functions + target_grip_height_in_steps (int): z axis steps distance from bottom of plate to grip the plate at target location + defaults to None + only used if transfer function is called from remove/replace_lid functions + incremental_lift (bool): True if you want to use incremental lift, False otherwise (default False) + incremental lift (good for ensuring lids are removed gently and correctly): + - grab plate at grip_height_in_steps + - raise 100 steps along z axis (repeat 5x) + - continue with rest of transfer + + Raises: + TODO + + Returns: + None """ # Extract the source and target location_types @@ -937,7 +887,6 @@ def transfer( Runs given function. """ s = PlateCrane("/dev/ttyUSB4") - # s.initialize() - # s.home() + diff --git a/src/platecrane_driver/resource_defs.py b/src/platecrane_driver/resource_defs.py index 8037b33..876c1c8 100644 --- a/src/platecrane_driver/resource_defs.py +++ b/src/platecrane_driver/resource_defs.py @@ -1,7 +1,7 @@ -from resource_types import Location, PlateResource -# from platecrane_driver.resource_types import Location, PlateResource # through WEI +# from resource_types import Location, PlateResource # when testing through driver +from platecrane_driver.resource_types import Location, PlateResource # through WEI -# Locations accessible by the PlateCrane EX +# Locations accessible by the PlateCrane EX. [R (base), Z (vertical axis), P (gripper rotation), Y (arm extension)] locations = { "Safe": Location(name="Safe", joint_angles=[182220, 2500, 460, -308], location_type="nest", safe_approach_height=0), "Stack1": Location(name="Stack1", joint_angles=[164672, -32703, 472, 5389], location_type="stack", safe_approach_height=0), @@ -27,4 +27,4 @@ "pcr_96well": PlateResource(plate_height = 0, grip_height=0, plate_height_with_lid=0, lid_height=0, lid_grip_height=0, lid_removal_grip_height=0), } -# R, Z, P, Y + From 49810a11534547240f7651553207d8fb88f71493 Mon Sep 17 00:00:00 2001 From: Ryan Lewis Date: Thu, 20 Jun 2024 16:48:18 -0500 Subject: [PATCH 12/19] WIP: cleanup from review --- .pre-commit-config.yaml | 6 +- Makefile | 3 - example.env | 5 +- pyproject.toml | 2 +- src/platecrane_driver/__init__.py | 1 + src/platecrane_driver/error_codes.py | 1 + src/platecrane_driver/plate_resources.json | 4 +- src/platecrane_driver/platecrane_driver.py | 376 +++++++++--------- .../platecrane_joint_limits.py | 1 + src/platecrane_driver/platecrane_locations.py | 2 +- src/platecrane_driver/resource_defs.py | 141 ++++++- src/platecrane_driver/resource_types.py | 13 +- src/platecrane_driver/sciclops_driver.py | 1 + src/platecrane_driver/serial_port.py | 1 + src/platecrane_driver/test.py | 13 +- src/platecrane_driver/types.py | 34 -- src/platecrane_rest_node.py | 2 +- 17 files changed, 346 insertions(+), 260 deletions(-) delete mode 100644 src/platecrane_driver/types.py diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 0e70236..db41200 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,6 +1,6 @@ repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.5.0 + rev: v4.6.0 hooks: - id: check-yaml - id: check-json @@ -12,12 +12,12 @@ repos: - id: end-of-file-fixer - id: trailing-whitespace - repo: https://github.com/kynan/nbstripout - rev: 0.6.1 + rev: 0.7.1 hooks: - id: nbstripout - repo: https://github.com/astral-sh/ruff-pre-commit # Ruff version. - rev: v0.1.10 + rev: v0.4.10 hooks: # Run the linter. - id: ruff diff --git a/Makefile b/Makefile index b7d26ba..0c84d78 100644 --- a/Makefile +++ b/Makefile @@ -1,14 +1,11 @@ # Python Configuration PYPROJECT_TOML := pyproject.toml -PROJECT_VERSION := $(shell grep -oP '(?<=version = ")[^"]+' $(PYPROJECT_TOML) | head -n 1) .DEFAULT_GOAL := init .PHONY += init paths checks test clean init: # Do the initial configuration of the project @test -e .env || cp example.env .env - @sed -i 's/^PROJECT_VERSION=.*/PROJECT_VERSION=$(PROJECT_VERSION)/' .env - @sed -i 's/^PROJECT_PATH=.*/PROJECT_PATH=$(shell pwd | sed 's/\//\\\//g')/' .env .env: init diff --git a/example.env b/example.env index 6f16d0b..abe66f8 100644 --- a/example.env +++ b/example.env @@ -1,10 +1,9 @@ # Note: all paths are relative to the docker compose file DEVICE=/dev/ttyUSB2 -PROJECT_PATH= -PROJECT_VERSION=1.2.0 +PROJECT_VERSION=1.3.0 WEI_DATA_DIR=~/.wei WORKCELL_FILENAME=test_workcell.yaml -WORKCELLS_DIR=${PROJECT_PATH}/tests/workcell_defs +WORKCELLS_DIR=./tests/workcell_defs IMAGE=ghcr.io/ad-sdl/hudson_platecrane_module DOCKERFILE=Dockerfile REDIS_DIR=~/.wei/redis diff --git a/pyproject.toml b/pyproject.toml index e67daeb..152ffb0 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,6 +1,6 @@ [project] name = "hudson_platecrane_module" -version = "1.0.0" +version = "1.3.0" description = "Driver for the Platecrane and Sciclops" authors = [ {name = "Ryan D. Lewis", email="ryan.lewis@anl.gov"}, diff --git a/src/platecrane_driver/__init__.py b/src/platecrane_driver/__init__.py index 4501616..e44b21d 100644 --- a/src/platecrane_driver/__init__.py +++ b/src/platecrane_driver/__init__.py @@ -1,2 +1,3 @@ """Driver for Hudson Robotics PlateCranes.""" + __version__ = "0.1.0" diff --git a/src/platecrane_driver/error_codes.py b/src/platecrane_driver/error_codes.py index 91d4229..2efa23c 100644 --- a/src/platecrane_driver/error_codes.py +++ b/src/platecrane_driver/error_codes.py @@ -1,4 +1,5 @@ """Defines exceptions for error codes returned by the plate crane controller.""" + from __future__ import annotations diff --git a/src/platecrane_driver/plate_resources.json b/src/platecrane_driver/plate_resources.json index 5c10cb1..5eaab88 100644 --- a/src/platecrane_driver/plate_resources.json +++ b/src/platecrane_driver/plate_resources.json @@ -9,7 +9,7 @@ }, "96_well":{ - "plate_above_height": 800, + "plate_above_height": 800, "plate_pick_steps_module": 1700, "plate_pick_steps_stack": 2700, "plate_lid_steps": 1300, @@ -49,7 +49,7 @@ "lid_destination_height": 0 }, "test_96_well":{ - "plate_above_height": 800, + "plate_above_height": 800, "plate_pick_steps_module": 1700, "plate_pick_steps_stack": 1500, "plate_lid_steps": 1300, diff --git a/src/platecrane_driver/platecrane_driver.py b/src/platecrane_driver/platecrane_driver.py index db0b61f..8ac4d75 100644 --- a/src/platecrane_driver/platecrane_driver.py +++ b/src/platecrane_driver/platecrane_driver.py @@ -1,19 +1,20 @@ """Handle Proper Interfacing with the PlateCrane""" -import json + import re -from pathlib import Path import time -from platecrane_driver.serial_port import SerialPort # use when running through WEI REST clients from platecrane_driver.resource_defs import locations, plate_definitions from platecrane_driver.resource_types import PlateResource +from platecrane_driver.serial_port import ( + SerialPort, # use when running through WEI REST clients +) # from serial_port import SerialPort # use when running through the driver # from resource_defs import locations, plate_definitions # from resource_types import PlateResource """ -# TODOs: +# TODOs: * combine two initialization functions * Look into how to slow speed of stack pick and place * should we be using error_codes.py to be doing some of the error checking/raising @@ -23,19 +24,20 @@ * Maybe create a plate detect function within pick stack plate function """ + class PlateCrane: - """ Python interface that allows remote commands to be executed to the plate_crane.""" + """Python interface that allows remote commands to be executed to the plate_crane.""" __serial_port: SerialPort def __init__(self, host_path="/dev/ttyUSB4", baud_rate=9600): """Initialization function - Args: + Args: host_path (str): usb path of PlateCrane EX device baud_rate (int): baud rate to use for communication with the PlateCrane EX device - Returns: + Returns: None """ @@ -62,10 +64,10 @@ def initialize(self): def home(self, timeout=28): """Homes all of the axes. Returns to neutral position (above exchange) - Args: + Args: timeout (int): Seconds to wait for plate crane response after sending serial command, defaults to 28 seconds. - - Returns: + + Returns: None """ @@ -106,20 +108,19 @@ def get_location_list(self): print("Error in get_status") self.robot_error = err - def get_location_joint_values(self, location: str = None) -> list: """Returns list of 4 joint values associated with a position name - Note: right now this returns the joint values stored in the + Note: right now this returns the joint values stored in the PlateCrane EX device memory, not the locations stored in resource_defs.py TODO: delete this function or associate it with resource_defs.py - Args: - location (str): Name of location + Args: + location (str): Name of location - Returns: + Returns: joint_values ([int]): [R, Z, P, Y] joint values - R (base rotation) - Z (arm vertical axis) @@ -136,11 +137,11 @@ def get_location_joint_values(self, location: str = None) -> list: def get_position(self) -> list: """Returns list of joint values for current position of the PlateCrane EX arm - - Args: + + Args: None - - Returns: + + Returns: current_position ([int]): [R, Z, P, Y] joint values - R (base rotation) - Z (arm vertical axis) @@ -150,16 +151,20 @@ def get_position(self) -> list: command = "GETPOS\r\n" - try: + try: # collect coordinates of current position - current_position = list(self.__serial_port.send_command(command,1).split(" ")) + current_position = list( + self.__serial_port.send_command(command, 1).split(" ") + ) print(current_position) current_position = [eval(x.strip(",")) for x in current_position] print(current_position) - except Exception: + except Exception: # Fall back: overlapping serial responses were detected. Wait 5 seconds then resend latest command time.sleep(5) - current_position = list(self.__serial_port.send_command(command,1).split(" ")) + current_position = list( + self.__serial_port.send_command(command, 1).split(" ") + ) current_position = [eval(x.strip(",")) for x in current_position] return current_position @@ -174,14 +179,14 @@ def set_location( ): """Saves a new location into PlateCrane EX device memory - Args: + Args: location_name (str): Name of location to be saved R (int): base rotation (units: motor steps) Z (int): vertical axis (units: motor steps) P (int): gripper rotation (units: motor steps) Y (int): arm extension (units: motor steps) - - Returns: + + Returns: None """ @@ -197,52 +202,50 @@ def set_location( def delete_location(self, location_name: str = None): """Deletes an existing location from the PlateCrane EX device memory - Args: - location_name (str): Name of location to delete + Args: + location_name (str): Name of location to delete - Returns: + Returns: None """ if not location_name: raise Exception("No location name provided") - command = "DELETEPOINT %s\r\n" % ( - location_name - ) + command = "DELETEPOINT %s\r\n" % (location_name) self.__serial_port.send_command(command) def gripper_open(self): """Opens gripper""" - command = "OPEN\r\n" + command = "OPEN\r\n" self.__serial_port.send_command(command) - + def gripper_close(self): """Closes gripper""" - command = "CLOSE\r\n" + command = "CLOSE\r\n" self.__serial_port.send_command(command) def check_open(self): """Checks if gripper is open""" - command = "GETGRIPPERISOPEN\r\n" + command = "GETGRIPPERISOPEN\r\n" self.__serial_port.send_command(command) def check_closed(self): """Checks if gripper is closed""" - command = "GETGRIPPERISCLOSED\r\n" + command = "GETGRIPPERISCLOSED\r\n" self.__serial_port.send_command(command) def jog(self, axis, distance) -> None: """Moves the specified axis the specified distance. - Args: + Args: axis (str): "R", "Z", "P", or "Y" distance (int): distance to move along the axis (units = motor steps) - Returns: + Returns: None """ @@ -252,7 +255,7 @@ def jog(self, axis, distance) -> None: def move_joint_angles(self, R: int, Z: int, P: int, Y: int) -> None: """Move to a specified location - Args: + Args: R (int): base rotation (unit = motor steps) Z (int): vertical axis (unit = motor steps) P (int): gripper rotation (unit = motor steps) @@ -277,19 +280,19 @@ def move_joint_angles(self, R: int, Z: int, P: int, Y: int) -> None: def move_single_axis(self, axis: str, loc: str, delay_time=1.5) -> None: """Moves on a single axis, using an existing location in PlateCrane EX device memory as reference - Args: + Args: axis (str): axis to move along loc (str): name of location in PlateCrane EX device memory to use as reference delay_time (float): serial command timeout. Seconds to wait before expecting a serial response defaults to 1.5 seconds - Raises: + Raises: TODO - - Returns: + + Returns: None - TODO: + TODO: * Handle errors using error_codes.py * Reference locations in resource_defs.py, not device memory """ @@ -306,15 +309,15 @@ def move_single_axis(self, axis: str, loc: str, delay_time=1.5) -> None: def move_location(self, loc: str = None, move_time: float = 4.7) -> None: """Moves all joint to the given location. - Args: + Args: loc (str): location to move to move_time (float): serial command timeout. Seconds to wait before expecting a serial response defaults to 4.7 seconds - Returns: - None + Returns: + None - TODO: + TODO: * Handle the error raising within error_codes.py """ if not loc: @@ -328,17 +331,17 @@ def move_location(self, loc: str = None, move_time: float = 4.7) -> None: def move_tower_neutral(self) -> None: """Moves the tower to neutral position - TODO: - * This still creates a TEMP position, moves to it, then deletes it after. - Change this, and other related methods below, to use only access + TODO: + * This still creates a TEMP position, moves to it, then deletes it after. + Change this, and other related methods below, to use only access locations in resource_defs """ current_pos = self.get_position() self.move_joint_angles( R=current_pos[0], Z=locations["Safe"].joint_angles[1], - P=current_pos[2], - Y=current_pos[3] + P=current_pos[2], + Y=current_pos[3], ) def move_arm_neutral(self) -> None: @@ -348,20 +351,20 @@ def move_arm_neutral(self) -> None: self.move_joint_angles( R=current_pos[0], Z=current_pos[1], - P=current_pos[2], + P=current_pos[2], Y=locations["Safe"].joint_angles[3], ) def move_gripper_neutral(self) -> None: """Moves the gripper to neutral position""" - + self.move_single_axis("P", "Safe", delay_time=0.3) current_pos = self.get_position() self.move_joint_angles( R=current_pos[0], Z=current_pos[1], - P=locations, + P=locations, Y=current_pos[3], ) @@ -371,19 +374,19 @@ def move_joints_neutral(self) -> None: self.move_tower_neutral() def pick_plate_safe_approach( - self, - source: str, + self, + source: str, plate_type: str, - grip_height_in_steps: int, + grip_height_in_steps: int, ) -> None: - """Picks a plate from a source type "nest" using a safe travel path. + """Picks a plate from a source type "nest" using a safe travel path. - Args: + Args: source (str): source location name defined in resource_defs.py plate_type (str): plate definition name defined in resource_defs.py grip_height_in_steps (int): z axis steps distance from bottom of plate to grip the plate - Returns: + Returns: None """ @@ -393,7 +396,7 @@ def pick_plate_safe_approach( # Rotate base (R axis) toward plate location current_pos = self.get_position() self.move_joint_angles( - R=locations[source].joint_angles[0], + R=locations[source].joint_angles[0], Z=current_pos[1], P=current_pos[2], Y=current_pos[3], @@ -402,25 +405,25 @@ def pick_plate_safe_approach( # Lower z axis to safe_approach_z height current_pos = self.get_position() self.move_joint_angles( - R=current_pos[0], + R=current_pos[0], Z=locations[plate_type].safe_approach_height, P=current_pos[2], Y=current_pos[3], ) - # extend arm over plate and rotate gripper to correct orientation + # extend arm over plate and rotate gripper to correct orientation current_pos = self.get_position() self.move_joint_angles( - R=current_pos[0], + R=current_pos[0], Z=current_pos[1], - P=locations[source].joint_angles[2], + P=locations[source].joint_angles[2], Y=locations[source].joint_angles[3], ) # Lower arm (z axis) to correct plate grip height current_pos = self.get_position() self.move_joint_angles( - R=current_pos[0], + R=current_pos[0], Z=locations[source].joint_angles[1] + grip_height_in_steps, P=current_pos[2], Y=current_pos[3], @@ -432,7 +435,7 @@ def pick_plate_safe_approach( # Move arm with plate back to safe approach height current_pos = self.get_position() self.move_joint_angles( - R=current_pos[0], + R=current_pos[0], Z=locations[source].safe_approach_height, P=current_pos[2], Y=current_pos[3], @@ -441,7 +444,7 @@ def pick_plate_safe_approach( # retract arm (Y axis) as much as possible (to same Y axis value as Safe location) current_pos = self.get_position() self.move_joint_angles( - R=current_pos[0], + R=current_pos[0], Z=current_pos[1], P=current_pos[2], Y=locations["Safe"].joint_angles[3], @@ -451,18 +454,18 @@ def pick_plate_safe_approach( self.move_arm_neutral() def place_plate_safe_approach( - self, - target: str, + self, + target: str, grip_height_in_steps: int, ) -> None: """Places a plate to a target location of type "nest" using a safe travel path. - Args: + Args: target (str): source location name defined in resource_defs.py plate_type (str): plate definition name defined in resource_defs.py grip_height_in_steps (int): z axis steps distance from bottom of plate to grip the plate - Returns: + Returns: None """ @@ -478,25 +481,25 @@ def place_plate_safe_approach( # Lower z axis to safe_approach_z height current_pos = self.get_position() self.move_joint_angles( - R=current_pos[0], + R=current_pos[0], Z=locations[target].safe_approach_height, P=current_pos[2], Y=current_pos[3], ) - # extend arm over plate and rotate gripper to correct orientation + # extend arm over plate and rotate gripper to correct orientation current_pos = self.get_position() self.move_joint_angles( - R=current_pos[0], + R=current_pos[0], Z=current_pos[1], - P=locations[target].joint_angles[2], + P=locations[target].joint_angles[2], Y=locations[target].joint_angles[3], ) # lower arm (z axis) to correct plate grip height current_pos = self.get_position() self.move_joint_angles( - R=current_pos[0], + R=current_pos[0], Z=locations[target].joint_angles[1] + grip_height_in_steps, P=current_pos[2], Y=current_pos[3], @@ -508,7 +511,7 @@ def place_plate_safe_approach( # Back away using safe approach path current_pos = self.get_position() self.move_joint_angles( - R=current_pos[0], + R=current_pos[0], Z=locations[target].safe_approach_height, P=current_pos[2], Y=current_pos[3], @@ -517,7 +520,7 @@ def place_plate_safe_approach( # retract arm (Y axis) as much as possible (to same Y axis value as Safe location) current_pos = self.get_position() self.move_joint_angles( - R=current_pos[0], + R=current_pos[0], Z=current_pos[1], P=current_pos[2], Y=locations["Safe"].joint_angles[3], @@ -528,41 +531,41 @@ def place_plate_safe_approach( self.move_arm_neutral() def pick_plate_direct( - self, - source: str, - source_type: str, - plate_type: str, - grip_height_in_steps: int, - has_lid: bool, - incremental_lift: bool=False + self, + source: str, + source_type: str, + plate_type: str, + grip_height_in_steps: int, + has_lid: bool, + incremental_lift: bool = False, ) -> None: """Picks a plate from a source location of type either "nest" or "stack" using a direct travel path "nest" transfers: gripper open, direct to grab plate z height "stack" transfers: gripper closed, touch top of plate, z up, z down to correct grab plate height - Args: + Args: source (str): source location name defined in resource_defs.py source_type (str): either "nest" or "stack" plate_type (str): plate definition name defined in resource_defs.py grip_height_in_steps (int): z axis steps distance from bottom of plate to grip the plate has_lid (bool): True if plate has lid, False otherwise incremental_lift (bool): True if you want to use incremental lift, False otherwise (default False) - incremental lift (good for ensuring lids are removed gently and correctly): + incremental lift (good for ensuring lids are removed gently and correctly): - grab plate at grip_height_in_steps - raise 100 steps along z axis (repeat 5x) - continue with rest of transfer - Returns: + Returns: None """ # Rotate R axis (base rotation) over the plate current_pos = self.get_position() self.move_joint_angles( - R=locations[source].joint_angles[0], - Z=current_pos[1], - P=current_pos[2], + R=locations[source].joint_angles[0], + Z=current_pos[1], + P=current_pos[2], Y=current_pos[3], ) @@ -572,39 +575,49 @@ def pick_plate_direct( # tap arm on top of plate stack to determine stack height self.move_joint_angles( - R=locations[source].joint_angles[0], - Z=locations[source].joint_angles[1], - P=locations[source].joint_angles[2], + R=locations[source].joint_angles[0], + Z=locations[source].joint_angles[1], + P=locations[source].joint_angles[2], Y=locations[source].joint_angles[3], ) # Move up, open gripper, grab plate at correct height - self.jog("Z", 1000) + self.jog("Z", 1000) self.gripper_open() # Calculate z travel from grip height with/without lid - if has_lid: - z_jog_down_from_plate_top = PlateResource.convert_to_steps(plate_definitions[plate_type].plate_height_with_lid) - grip_height_in_steps - else: - z_jog_down_from_plate_top = PlateResource.convert_to_steps(plate_definitions[plate_type].plate_height) - grip_height_in_steps + if has_lid: + z_jog_down_from_plate_top = ( + PlateResource.convert_to_steps( + plate_definitions[plate_type].plate_height_with_lid + ) + - grip_height_in_steps + ) + else: + z_jog_down_from_plate_top = ( + PlateResource.convert_to_steps( + plate_definitions[plate_type].plate_height + ) + - grip_height_in_steps + ) - # Move down to correct z height to grip plate + # Move down to correct z height to grip plate self.jog("Z", -(1000 + z_jog_down_from_plate_top)) - - else: # if source_type == nest: + + else: # if source_type == nest: self.gripper_open() - + self.move_joint_angles( - R=locations[source].joint_angles[0], - Z=locations[source].joint_angles[1] + grip_height_in_steps, - P=locations[source].joint_angles[2], + R=locations[source].joint_angles[0], + Z=locations[source].joint_angles[1] + grip_height_in_steps, + P=locations[source].joint_angles[2], Y=locations[source].joint_angles[3], ) - + # open the gripper self.gripper_close() - if incremental_lift: + if incremental_lift: self.jog("Z", 100) self.jog("Z", 100) self.jog("Z", 100) @@ -615,25 +628,24 @@ def pick_plate_direct( self.move_tower_neutral() self.move_arm_neutral() - def place_plate_direct( - self, - target: str, - target_type: str, # TODO: use later to slow speed for target_type = "stack" - grip_height_in_steps: str, - ) -> None: + self, + target: str, + target_type: str, # TODO: use later to slow speed for target_type = "stack" + grip_height_in_steps: str, + ) -> None: """Places a plate onto a target location of type either "nest" or "stack" using a direct travel path - Args: + Args: target (str): target location name defined in resource_defs.py target_type (str): either "nest" or "stack" plate_type (str): plate definition name defined in resource_defs.py grip_height_in_steps (int): z axis steps distance from bottom of plate to grip the plate - Returns: + Returns: None - TODO: + TODO: * use target_type variable to slow approach in "stack" transfers to avoid striking other plates """ @@ -656,7 +668,7 @@ def place_plate_direct( ) # Lower arm (z axis) to plate grip height - current_pos=self.get_position() + current_pos = self.get_position() self.move_joint_angles( R=current_pos[0], Z=locations[target].joint_angles[1] + grip_height_in_steps, @@ -671,11 +683,11 @@ def place_plate_direct( def _is_location_joint_values(self, location: str, name: str = "temp") -> str: """ - If the location was provided as joint values, transfer joint values into a saved location - on the robot and return the location name. If location parameter is a name of an already saved + If the location was provided as joint values, transfer joint values into a saved location + on the robot and return the location name. If location parameter is a name of an already saved location, do nothing. - TODO: + TODO: * Is there any reason we should keep this function? """ try: @@ -704,22 +716,28 @@ def remove_lid( ) -> None: """Removes lid from a plate at source location and places lid at target location - Args: + Args: source (str): source location name defined in resource_defs.py target (str): target location name defined in resource_defs.py plate_type (str): plate definition name defined in resource_defs.py height_offset (int): change in z height to be applied to grip location on the lid (units = mm) defaults to 0mm - Returns: + Returns: None """ - # Calculate grip height in motor steps - source_grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].lid_removal_grip_height + height_offset) - target_grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].plate_height_with_lid - plate_definitions[plate_type].lid_height + height_offset) + # Calculate grip height in motor steps + source_grip_height_in_steps = PlateResource.convert_to_steps( + plate_definitions[plate_type].lid_removal_grip_height + height_offset + ) + target_grip_height_in_steps = PlateResource.convert_to_steps( + plate_definitions[plate_type].plate_height_with_lid + - plate_definitions[plate_type].lid_height + + height_offset + ) - # Pass to transfer function but specify that it is a lid we're transferring + # Pass to transfer function but specify that it is a lid we're transferring self.transfer( source=source, target=target, @@ -728,7 +746,7 @@ def remove_lid( is_lid=True, source_grip_height_in_steps=source_grip_height_in_steps, target_grip_height_in_steps=target_grip_height_in_steps, - incremental_lift = True, + incremental_lift=True, ) def replace_lid( @@ -738,23 +756,27 @@ def replace_lid( plate_type: str, height_offset: int = 0, ) -> None: - """"Replaces lid at source location onto a plate at the target location + """ "Replaces lid at source location onto a plate at the target location - Args: + Args: source (str): source location name defined in resource_defs.py target (str): target location name defined in resource_defs.py plate_type (str): plate definition name defined in resource_defs.py height_offset (int): change in z height to be applied to grip location on the lid (units = mm) defaults to 0mm - Returns: + Returns: None """ # Calculate grip height in motor steps - source_grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].lid_grip_height + height_offset) - target_grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].lid_removal_grip_height + height_offset) + source_grip_height_in_steps = PlateResource.convert_to_steps( + plate_definitions[plate_type].lid_grip_height + height_offset + ) + target_grip_height_in_steps = PlateResource.convert_to_steps( + plate_definitions[plate_type].lid_removal_grip_height + height_offset + ) - # Pass to transfer function but specify that it is a lid we're transferring + # Pass to transfer function but specify that it is a lid we're transferring self.transfer( source=source, target=target, @@ -770,7 +792,7 @@ def transfer( source: str, target: str, plate_type: str, - height_offset: int = 0, # units = mm + height_offset: int = 0, # units = mm is_lid: bool = False, has_lid: bool = False, source_grip_height_in_steps: int = None, # if removing/replacing lid @@ -779,12 +801,12 @@ def transfer( ) -> None: """Handles the transfer request - Args: + Args: source (str): source location name defined in resource_defs.py target (str): target location name defined in resource_defs.py plate_type (str): plate definition name defined in resource_defs.py height_offset (int): change in z height to be applied to grip location on the lid (units = mm) - defaults to 0mm + defaults to 0mm is_lid (bool): True if transferring a lid, False otherwise defaults to False has_lid (bool): True if plate being transferred has a lid, otherwise False @@ -796,97 +818,95 @@ def transfer( defaults to None only used if transfer function is called from remove/replace_lid functions incremental_lift (bool): True if you want to use incremental lift, False otherwise (default False) - incremental lift (good for ensuring lids are removed gently and correctly): + incremental lift (good for ensuring lids are removed gently and correctly): - grab plate at grip_height_in_steps - raise 100 steps along z axis (repeat 5x) - continue with rest of transfer - Raises: + Raises: TODO - - Returns: - None + + Returns: + None """ # Extract the source and target location_types source_type = locations[source].location_type target_type = locations[target].location_type - + # Determine source and target grip heights from bottom of plate (converted from mm to z motor steps) - """If the transfer function is called from either remove_lid() or replace_lid(), + """If the transfer function is called from either remove_lid() or replace_lid(), these values will be precalculated and passed in. Otherwise they need to be calculated here.""" - if not is_lid: - grip_height_in_steps = PlateResource.convert_to_steps(plate_definitions[plate_type].grip_height + height_offset) + if not is_lid: + grip_height_in_steps = PlateResource.convert_to_steps( + plate_definitions[plate_type].grip_height + height_offset + ) source_grip_height_in_steps = grip_height_in_steps target_grip_height_in_steps = grip_height_in_steps # is safe approach required for source and/or target? - source_use_safe_approach = False if locations[source].safe_approach_height == 0 else True - target_use_safe_approach = False if locations[target].safe_approach_height == 0 else True + source_use_safe_approach = ( + False if locations[source].safe_approach_height == 0 else True + ) + target_use_safe_approach = ( + False if locations[target].safe_approach_height == 0 else True + ) # PICK PLATE FROM SOURCE LOCATION if source_type == "stack": self.pick_plate_direct( source=source, - source_type=source_type, # "stack" + source_type=source_type, # "stack" plate_type=plate_type, grip_height_in_steps=source_grip_height_in_steps, has_lid=has_lid, - incremental_lift=incremental_lift + incremental_lift=incremental_lift, ) - elif source_type == "nest": + elif source_type == "nest": if source_use_safe_approach: self.pick_plate_safe_approach( - source=source, - plate_type=plate_type, + source=source, + plate_type=plate_type, grip_height_in_steps=source_grip_height_in_steps, ) else: self.pick_plate_direct( - source=source, + source=source, source_type=source_type, # nest plate_type=plate_type, grip_height_in_steps=source_grip_height_in_steps, has_lid=has_lid, incremental_lift=incremental_lift, ) - else: - raise Exception( - "Source location type not defined correctly" - ) - + else: + raise Exception("Source location type not defined correctly") # PLACE PLATE AT TARGET LOCATION - if target_type == "stack": + if target_type == "stack": self.place_plate_direct( - target=target, + target=target, target_type=target_type, grip_height_in_steps=target_grip_height_in_steps, - ) - elif target_type == "nest": - if target_use_safe_approach: + ) + elif target_type == "nest": + if target_use_safe_approach: self.place_plate_safe_approach( target=target, grip_height_in_steps=target_grip_height_in_steps, ) - else: + else: self.place_plate_direct( target=target, target_type=target_type, grip_height_in_steps=target_grip_height_in_steps, ) - else: - raise Exception( - "Target location type not defined correctly" - ) - - + else: + raise Exception("Target location type not defined correctly") + + if __name__ == "__main__": """ Runs given function. """ s = PlateCrane("/dev/ttyUSB4") - - - diff --git a/src/platecrane_driver/platecrane_joint_limits.py b/src/platecrane_driver/platecrane_joint_limits.py index 50f9c02..55217bb 100644 --- a/src/platecrane_driver/platecrane_joint_limits.py +++ b/src/platecrane_driver/platecrane_joint_limits.py @@ -1,4 +1,5 @@ """Joint limits for the platecrane.""" + platecrane_joint_limits = { "R": [-1200, 10200], "Z": [-13600, 300], diff --git a/src/platecrane_driver/platecrane_locations.py b/src/platecrane_driver/platecrane_locations.py index f86bce2..a7f3583 100644 --- a/src/platecrane_driver/platecrane_locations.py +++ b/src/platecrane_driver/platecrane_locations.py @@ -1,4 +1,4 @@ -""" THIS IS JUST FOR REFERENCE AND IT IS NOT INTEGRATED WITH THE DRIVER""" +"""THIS IS JUST FOR REFERENCE AND IT IS NOT INTEGRATED WITH THE DRIVER""" platecrane_locations = { "Safe": "117902 2349 -5882 0", diff --git a/src/platecrane_driver/resource_defs.py b/src/platecrane_driver/resource_defs.py index 876c1c8..d813b2f 100644 --- a/src/platecrane_driver/resource_defs.py +++ b/src/platecrane_driver/resource_defs.py @@ -1,30 +1,129 @@ +"""Resource Definitions for the PlateCrane in BIO 350""" + # from resource_types import Location, PlateResource # when testing through driver -from platecrane_driver.resource_types import Location, PlateResource # through WEI +from platecrane_driver.resource_types import ( + Location, # through WEI + PlateResource, +) # Locations accessible by the PlateCrane EX. [R (base), Z (vertical axis), P (gripper rotation), Y (arm extension)] locations = { - "Safe": Location(name="Safe", joint_angles=[182220, 2500, 460, -308], location_type="nest", safe_approach_height=0), - "Stack1": Location(name="Stack1", joint_angles=[164672, -32703, 472, 5389], location_type="stack", safe_approach_height=0), - "Stack2": Location(name="Stack2", joint_angles=[182220, -32703, 460, 5420], location_type="stack", safe_approach_height=0), - "Stack3": Location(name="Stack3", joint_angles=[199708, -32703, 514, 5484], location_type="stack", safe_approach_height=0), - "Stack4": Location(name="Stack4", joint_angles=[217401, -32703, 546, 5473], location_type="stack", safe_approach_height=0), - "Stack5": Location(name="Stack5", joint_angles=[235104, -32703, 532, 5453], location_type="stack", safe_approach_height=0), - "LidNest1": Location(name="LidNest1", joint_angles=[168355, -31800, 484, -306], location_type="nest", safe_approach_height=0), - "LidNest2": Location(name="LidNest2", joint_angles=[199805, -31800, 484, -306], location_type="nest", safe_approach_height=0), - "LidNest3": Location(name="LidNest3", joint_angles=[231449, -31800, 484, -306], location_type="nest", safe_approach_height=0), - "Solo.Position1": Location(name="Solo.Position1", joint_angles=[36703, -27951, -1000, 3630], location_type="nest", safe_approach_height=0), - "Solo.Position2": Location(name="Solo.Position2", joint_angles=[53182, -27951, -413, 834], location_type="nest", safe_approach_height=0), - "Hidex.Nest": Location(name="Hidex.Nest", joint_angles=[102327, -31090, -5830, 2378], location_type="nest", safe_approach_height=-27033), - "Sealer.Nest": Location(name="Sealer.Nest", joint_angles=[117305, 920, -4766, 4550], location_type="nest", safe_approach_height=0), - "Peeler.Nest": Location(name="Peeler.Nest", joint_angles=[292635, -31058, -4521, 4235], location_type="nest", safe_approach_height=0), - "Liconic.Nest": Location(name="Liconic.Nest", joint_angles=[265603, -19900, -5413, 4953], location_type="nest", safe_approach_height=0), + "Safe": Location( + name="Safe", + joint_angles=[182220, 2500, 460, -308], + location_type="nest", + safe_approach_height=0, + ), + "Stack1": Location( + name="Stack1", + joint_angles=[164672, -32703, 472, 5389], + location_type="stack", + safe_approach_height=0, + ), + "Stack2": Location( + name="Stack2", + joint_angles=[182220, -32703, 460, 5420], + location_type="stack", + safe_approach_height=0, + ), + "Stack3": Location( + name="Stack3", + joint_angles=[199708, -32703, 514, 5484], + location_type="stack", + safe_approach_height=0, + ), + "Stack4": Location( + name="Stack4", + joint_angles=[217401, -32703, 546, 5473], + location_type="stack", + safe_approach_height=0, + ), + "Stack5": Location( + name="Stack5", + joint_angles=[235104, -32703, 532, 5453], + location_type="stack", + safe_approach_height=0, + ), + "LidNest1": Location( + name="LidNest1", + joint_angles=[168355, -31800, 484, -306], + location_type="nest", + safe_approach_height=0, + ), + "LidNest2": Location( + name="LidNest2", + joint_angles=[199805, -31800, 484, -306], + location_type="nest", + safe_approach_height=0, + ), + "LidNest3": Location( + name="LidNest3", + joint_angles=[231449, -31800, 484, -306], + location_type="nest", + safe_approach_height=0, + ), + "Solo.Position1": Location( + name="Solo.Position1", + joint_angles=[36703, -27951, -1000, 3630], + location_type="nest", + safe_approach_height=0, + ), + "Solo.Position2": Location( + name="Solo.Position2", + joint_angles=[53182, -27951, -413, 834], + location_type="nest", + safe_approach_height=0, + ), + "Hidex.Nest": Location( + name="Hidex.Nest", + joint_angles=[102327, -31090, -5830, 2378], + location_type="nest", + safe_approach_height=-27033, + ), + "Sealer.Nest": Location( + name="Sealer.Nest", + joint_angles=[117305, 920, -4766, 4550], + location_type="nest", + safe_approach_height=0, + ), + "Peeler.Nest": Location( + name="Peeler.Nest", + joint_angles=[292635, -31058, -4521, 4235], + location_type="nest", + safe_approach_height=0, + ), + "Liconic.Nest": Location( + name="Liconic.Nest", + joint_angles=[265603, -19900, -5413, 4953], + location_type="nest", + safe_approach_height=0, + ), } # Dimensions of labware used on the BIO_Workcells plate_definitions = { - "flat_bottom_96well": PlateResource(plate_height = 14, grip_height=3, plate_height_with_lid=16, lid_height=10, lid_grip_height=4, lid_removal_grip_height=12), - "tip_box_180uL": PlateResource(plate_height = 0, grip_height=0, plate_height_with_lid=0, lid_height=0, lid_grip_height=0, lid_removal_grip_height=0), - "pcr_96well": PlateResource(plate_height = 0, grip_height=0, plate_height_with_lid=0, lid_height=0, lid_grip_height=0, lid_removal_grip_height=0), + "flat_bottom_96well": PlateResource( + plate_height=14, + grip_height=3, + plate_height_with_lid=16, + lid_height=10, + lid_grip_height=4, + lid_removal_grip_height=12, + ), + "tip_box_180uL": PlateResource( + plate_height=0, + grip_height=0, + plate_height_with_lid=0, + lid_height=0, + lid_grip_height=0, + lid_removal_grip_height=0, + ), + "pcr_96well": PlateResource( + plate_height=0, + grip_height=0, + plate_height_with_lid=0, + lid_height=0, + lid_grip_height=0, + lid_removal_grip_height=0, + ), } - - diff --git a/src/platecrane_driver/resource_types.py b/src/platecrane_driver/resource_types.py index d5c798a..68170aa 100644 --- a/src/platecrane_driver/resource_types.py +++ b/src/platecrane_driver/resource_types.py @@ -1,8 +1,13 @@ +"""This module contains the Pydantic models for the PlateCrane resource types""" + from typing import List, Optional + from pydantic import BaseModel + class Location(BaseModel): - + """A location accessible by the PlateCrane EX""" + name: str """Internal name of the location""" joint_angles: List[int] @@ -15,6 +20,7 @@ class Location(BaseModel): class PlateResource(BaseModel): + """A plate resource that can be manipulated by the PlateCrane EX""" # Plate Properties @@ -35,9 +41,8 @@ class PlateResource(BaseModel): lid_removal_grip_height: Optional[float] = None """The height at which to grip the lid when removing it, measured from the bottom of the lidded plate""" - def convert_to_steps(plate_measurement_in_mm: float) -> int: - """Converts plate measurements in mm to PlateCrane EX motor steps on the z-axis """ + def convert_to_steps(plate_measurement_in_mm: float) -> int: + """Converts plate measurements in mm to PlateCrane EX motor steps on the z-axis""" steps_per_mm = 80.5 steps = int(plate_measurement_in_mm * steps_per_mm) return steps - diff --git a/src/platecrane_driver/sciclops_driver.py b/src/platecrane_driver/sciclops_driver.py index 7e7d80f..1bd7356 100644 --- a/src/platecrane_driver/sciclops_driver.py +++ b/src/platecrane_driver/sciclops_driver.py @@ -1,4 +1,5 @@ """Driver for the Hudson Robotics Sciclops robot.""" + import asyncio import re diff --git a/src/platecrane_driver/serial_port.py b/src/platecrane_driver/serial_port.py index 4d57b38..30108ce 100644 --- a/src/platecrane_driver/serial_port.py +++ b/src/platecrane_driver/serial_port.py @@ -1,4 +1,5 @@ """Provides SerialPort class to interface with the plate_crane.""" + import time from serial import Serial, SerialException diff --git a/src/platecrane_driver/test.py b/src/platecrane_driver/test.py index dfc8da8..3abee01 100644 --- a/src/platecrane_driver/test.py +++ b/src/platecrane_driver/test.py @@ -20,23 +20,18 @@ # print(s.lid_height) - # s.home() - - - print(s.get_location_list()) - #s.set_location("Solo.Position2", 53182, -27797, -413, 834) + # s.set_location("Solo.Position2", 53182, -27797, -413, 834) # s.set_location("HidexSafe", 209959, -2500, 490, -262) - #s.transfer("Solo.Position2","Solo.Position2",source_type="stack",target_type="stack", plate_type="96_well", height_offset=-200) # works if don't specify plate type (picks up lower) + # s.transfer("Solo.Position2","Solo.Position2",source_type="stack",target_type="stack", plate_type="96_well", height_offset=-200) # works if don't specify plate type (picks up lower) # s.transfer("Stack1","Solo.Position2",source_type="stack",target_type="stack", plate_type="96_well", height_offset=-250) # works if don't specify plate type (picks up lower) # s.remove_lid("Solo.Position2", "LidNest1", plate_type="96_well", height_offset=-650) # s.transfer("Solo.Position2","Solo.Position1", source_type="stack", target_type="module", plate_type="96_well", height_offset=-250) - #s.transfer("Stack1","Solo.Position2",source_type="stack",target_type="stack", plate_type="96_well") # doesn't work with plate type through driver (picks up higher) - #s.get_position() - + # s.transfer("Stack1","Solo.Position2",source_type="stack",target_type="stack", plate_type="96_well") # doesn't work with plate type through driver (picks up higher) + # s.get_position() # s.remove_lid(source="Solo.Position2", target="LidNest1", plate_type="test_96_well", height_offset = -100) # print(s.lid_height) diff --git a/src/platecrane_driver/types.py b/src/platecrane_driver/types.py deleted file mode 100644 index 77be10f..0000000 --- a/src/platecrane_driver/types.py +++ /dev/null @@ -1,34 +0,0 @@ -from typing import List, Optional -from pydantic import BaseModel - -class Location(BaseModel): - - name: str - """Internal name of the location""" - joint_angles: List[int] - """List of 4 joint angles (unit: integer stepper values)""" - safe_approach_height: Optional[int] = None - """A safe height (unit: integer stepper value for Z axis) from which - to extend the arm when approaching this location.""" - - -class PlateResource(BaseModel): - - # Plate Properties - - plate_height: float - """The height measured from the bottom of the plate to the top""" - grip_height: float - """The height at which to grip the plate, measured from the bottom of the plate""" - plate_height_with_lid: Optional[float] = None - """The height of the plate when lidded, measured from the bottom of the plate to the top of the lid. - Only required if the resource supports lids""" - - # Lid Properties - - lid_height: Optional[float] = None - """The height of the lid alone, measured from the bottom of the lid to the top of the lid""" - lid_grip_height: Optional[float] = None - """The height at which to grip the lid itself, measured from the bottom of the lid""" - lid_removal_grip_height: Optional[float] = None - """The height at which to grip the lid when removing it, measured from the bottom of the lidded plate""" diff --git a/src/platecrane_rest_node.py b/src/platecrane_rest_node.py index 34d73b2..0aa0827 100644 --- a/src/platecrane_rest_node.py +++ b/src/platecrane_rest_node.py @@ -218,7 +218,7 @@ def do_action(action_handle: str, action_vars): print("Source location: " + str(source)) target = action_args.get("target") print("Target location: " + str(target)) - plate_type = action_args.get("plate_type") + plate_type = action_args.get("plate_type") print("Plate type: " + str(plate_type)) height_offset = action_args.get("height_offset", 0) print("Height Offset: " + str(height_offset)) From eac2338da545503433718af757f1050949072738 Mon Sep 17 00:00:00 2001 From: Ryan Lewis Date: Mon, 24 Jun 2024 10:02:16 -0500 Subject: [PATCH 13/19] Pin to v0.5.8 for now --- Dockerfile | 2 +- src/platecrane_rest_node.py | 10 ---------- tests/test_module.py | 4 ++-- 3 files changed, 3 insertions(+), 13 deletions(-) diff --git a/Dockerfile b/Dockerfile index e0d2fb8..0e9afe2 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,4 +1,4 @@ -FROM ghcr.io/ad-sdl/wei +FROM ghcr.io/ad-sdl/wei:v0.5.8 LABEL org.opencontainers.image.source=https://github.com/AD-SDL/hudson_platecrane_module LABEL org.opencontainers.image.description="Drivers and REST API's for the Hudson Platecrane and Sciclops robots" diff --git a/src/platecrane_rest_node.py b/src/platecrane_rest_node.py index 0aa0827..5dbc9f1 100644 --- a/src/platecrane_rest_node.py +++ b/src/platecrane_rest_node.py @@ -228,16 +228,6 @@ def do_action(action_handle: str, action_vars): if action_handle == "transfer": print("Starting the transfer request") - # source_type = action_args.get("source_type", None) - # print("Source Type: " + str(source_type)) - - # target_type = action_args.get("target_type", None) - # print("Target Type: " + str(target_type)) - - # if not source_type or not target_type: - # print("Please provide source and target transfer types!") - # state = ModuleStatus.ERROR - try: platecrane.transfer( source, diff --git a/tests/test_module.py b/tests/test_module.py index afa955e..4968814 100644 --- a/tests/test_module.py +++ b/tests/test_module.py @@ -7,7 +7,7 @@ import pytest import requests from wei import ExperimentClient -from wei.core.data_classes import ModuleAbout, WorkcellData, WorkflowStatus +from wei.core.data_classes import ModuleAbout, Workcell, WorkflowStatus class TestWEI_Base(unittest.TestCase): @@ -20,7 +20,7 @@ def __init__(self, *args, **kwargs): self.workcell_file = self.root_dir / Path( "tests/workcell_defs/test_workcell.yaml" ) - self.workcell = WorkcellData.from_yaml(self.workcell_file) + self.workcell = Workcell.from_yaml(self.workcell_file) self.server_host = self.workcell.config.server_host self.server_port = self.workcell.config.server_port self.url = f"http://{self.server_host}:{self.server_port}" From 2820074ff7f0efb9aa1c4e27f548cb6d734d0f16 Mon Sep 17 00:00:00 2001 From: caseystone Date: Tue, 25 Jun 2024 17:51:45 -0500 Subject: [PATCH 14/19] added plate crane set speed method --- src/platecrane_driver/platecrane_driver.py | 36 +++++-- src/platecrane_driver/resource_defs.py | 114 +++------------------ 2 files changed, 47 insertions(+), 103 deletions(-) diff --git a/src/platecrane_driver/platecrane_driver.py b/src/platecrane_driver/platecrane_driver.py index 8ac4d75..4de43d2 100644 --- a/src/platecrane_driver/platecrane_driver.py +++ b/src/platecrane_driver/platecrane_driver.py @@ -9,6 +9,9 @@ SerialPort, # use when running through WEI REST clients ) +# from serial_port import SerialPort # use when running through the driver +# from resource_defs import locations, plate_definitions +# from resource_types import PlateResource # from serial_port import SerialPort # use when running through the driver # from resource_defs import locations, plate_definitions # from resource_types import PlateResource @@ -389,6 +392,8 @@ def pick_plate_safe_approach( Returns: None """ + print("PICK PLATE SAFE APPROACH CALLED") + # open the gripper self.gripper_open() @@ -402,11 +407,21 @@ def pick_plate_safe_approach( Y=current_pos[3], ) + # Rotate gripper + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=current_pos[1], + P=locations[source].joint_angles[2], + Y=current_pos[3], + ) + + # Lower z axis to safe_approach_z height current_pos = self.get_position() self.move_joint_angles( - R=current_pos[0], - Z=locations[plate_type].safe_approach_height, + R=current_pos[0], + Z=locations[source].safe_approach_height, P=current_pos[2], Y=current_pos[3], ) @@ -416,7 +431,7 @@ def pick_plate_safe_approach( self.move_joint_angles( R=current_pos[0], Z=current_pos[1], - P=locations[source].joint_angles[2], + P=current_pos[2], Y=locations[source].joint_angles[3], ) @@ -451,6 +466,7 @@ def pick_plate_safe_approach( ) # move rest of joints to neutral location + self.move_tower_neutral() self.move_arm_neutral() def place_plate_safe_approach( @@ -478,6 +494,15 @@ def place_plate_safe_approach( Y=current_pos[3], ) + # Rotate gripper to correct orientation + current_pos = self.get_position() + self.move_joint_angles( + R=current_pos[0], + Z=current_pos[1], + P=locations[target].joint_angles[2], + Y=current_pos[3], + ) + # Lower z axis to safe_approach_z height current_pos = self.get_position() self.move_joint_angles( @@ -487,12 +512,12 @@ def place_plate_safe_approach( Y=current_pos[3], ) - # extend arm over plate and rotate gripper to correct orientation + # extend arm over plate current_pos = self.get_position() self.move_joint_angles( R=current_pos[0], Z=current_pos[1], - P=locations[target].joint_angles[2], + P=current_pos[2], Y=locations[target].joint_angles[3], ) @@ -505,7 +530,6 @@ def place_plate_safe_approach( Y=current_pos[3], ) - time.sleep(2) self.gripper_open() # Back away using safe approach path diff --git a/src/platecrane_driver/resource_defs.py b/src/platecrane_driver/resource_defs.py index d813b2f..f1d7655 100644 --- a/src/platecrane_driver/resource_defs.py +++ b/src/platecrane_driver/resource_defs.py @@ -1,103 +1,23 @@ -"""Resource Definitions for the PlateCrane in BIO 350""" - -# from resource_types import Location, PlateResource # when testing through driver -from platecrane_driver.resource_types import ( - Location, # through WEI - PlateResource, -) +# from resource_types import Location, PlateResource # through driver +from platecrane_driver.resource_types import Location, PlateResource # through WEI # Locations accessible by the PlateCrane EX. [R (base), Z (vertical axis), P (gripper rotation), Y (arm extension)] locations = { - "Safe": Location( - name="Safe", - joint_angles=[182220, 2500, 460, -308], - location_type="nest", - safe_approach_height=0, - ), - "Stack1": Location( - name="Stack1", - joint_angles=[164672, -32703, 472, 5389], - location_type="stack", - safe_approach_height=0, - ), - "Stack2": Location( - name="Stack2", - joint_angles=[182220, -32703, 460, 5420], - location_type="stack", - safe_approach_height=0, - ), - "Stack3": Location( - name="Stack3", - joint_angles=[199708, -32703, 514, 5484], - location_type="stack", - safe_approach_height=0, - ), - "Stack4": Location( - name="Stack4", - joint_angles=[217401, -32703, 546, 5473], - location_type="stack", - safe_approach_height=0, - ), - "Stack5": Location( - name="Stack5", - joint_angles=[235104, -32703, 532, 5453], - location_type="stack", - safe_approach_height=0, - ), - "LidNest1": Location( - name="LidNest1", - joint_angles=[168355, -31800, 484, -306], - location_type="nest", - safe_approach_height=0, - ), - "LidNest2": Location( - name="LidNest2", - joint_angles=[199805, -31800, 484, -306], - location_type="nest", - safe_approach_height=0, - ), - "LidNest3": Location( - name="LidNest3", - joint_angles=[231449, -31800, 484, -306], - location_type="nest", - safe_approach_height=0, - ), - "Solo.Position1": Location( - name="Solo.Position1", - joint_angles=[36703, -27951, -1000, 3630], - location_type="nest", - safe_approach_height=0, - ), - "Solo.Position2": Location( - name="Solo.Position2", - joint_angles=[53182, -27951, -413, 834], - location_type="nest", - safe_approach_height=0, - ), - "Hidex.Nest": Location( - name="Hidex.Nest", - joint_angles=[102327, -31090, -5830, 2378], - location_type="nest", - safe_approach_height=-27033, - ), - "Sealer.Nest": Location( - name="Sealer.Nest", - joint_angles=[117305, 920, -4766, 4550], - location_type="nest", - safe_approach_height=0, - ), - "Peeler.Nest": Location( - name="Peeler.Nest", - joint_angles=[292635, -31058, -4521, 4235], - location_type="nest", - safe_approach_height=0, - ), - "Liconic.Nest": Location( - name="Liconic.Nest", - joint_angles=[265603, -19900, -5413, 4953], - location_type="nest", - safe_approach_height=0, - ), + "Safe": Location(name="Safe", joint_angles=[182220, 2500, 460, -308], location_type="nest", safe_approach_height=0), + "Stack1": Location(name="Stack1", joint_angles=[164672, -32703, 472, 5389], location_type="stack", safe_approach_height=0), + "Stack2": Location(name="Stack2", joint_angles=[182220, -32703, 460, 5420], location_type="stack", safe_approach_height=0), + "Stack3": Location(name="Stack3", joint_angles=[199708, -32703, 514, 5484], location_type="stack", safe_approach_height=0), + "Stack4": Location(name="Stack4", joint_angles=[217401, -32703, 546, 5473], location_type="stack", safe_approach_height=0), + "Stack5": Location(name="Stack5", joint_angles=[235104, -32703, 532, 5453], location_type="stack", safe_approach_height=0), + "LidNest1": Location(name="LidNest1", joint_angles=[168355, -31800, 484, -306], location_type="nest", safe_approach_height=0), + "LidNest2": Location(name="LidNest2", joint_angles=[199805, -31800, 484, -306], location_type="nest", safe_approach_height=0), + "LidNest3": Location(name="LidNest3", joint_angles=[231449, -31800, 484, -306], location_type="nest", safe_approach_height=0), + "Solo.Position1": Location(name="Solo.Position1", joint_angles=[36703, -27951, -1000, 3630], location_type="nest", safe_approach_height=0), + "Solo.Position2": Location(name="Solo.Position2", joint_angles=[53182, -27951, -413, 834], location_type="nest", safe_approach_height=0), + "Hidex.Nest": Location(name="Hidex.Nest", joint_angles=[102327, -31090, -5840, 2389], location_type="nest", safe_approach_height=-27033), + "Sealer.Nest": Location(name="Sealer.Nest", joint_angles=[117412, 920, -4766, 4514], location_type="nest", safe_approach_height=0), + "Peeler.Nest": Location(name="Peeler.Nest", joint_angles=[292635, -31008, -4521, 4235], location_type="nest", safe_approach_height=0), + "Liconic.Nest": Location(name="Liconic.Nest", joint_angles=[265563, -19800, -5413, 4978], location_type="nest", safe_approach_height=0), } # Dimensions of labware used on the BIO_Workcells From 7f9366f6e287aaf0a8236107483fab7082e5ceb0 Mon Sep 17 00:00:00 2001 From: caseystone Date: Tue, 25 Jun 2024 17:57:38 -0500 Subject: [PATCH 15/19] Added plate crane set speed function, for real this time --- src/platecrane_driver/platecrane_driver.py | 34 ++++++++++++++-------- 1 file changed, 22 insertions(+), 12 deletions(-) diff --git a/src/platecrane_driver/platecrane_driver.py b/src/platecrane_driver/platecrane_driver.py index 4de43d2..8bda731 100644 --- a/src/platecrane_driver/platecrane_driver.py +++ b/src/platecrane_driver/platecrane_driver.py @@ -3,18 +3,16 @@ import re import time -from platecrane_driver.resource_defs import locations, plate_definitions -from platecrane_driver.resource_types import PlateResource -from platecrane_driver.serial_port import ( - SerialPort, # use when running through WEI REST clients -) - -# from serial_port import SerialPort # use when running through the driver -# from resource_defs import locations, plate_definitions -# from resource_types import PlateResource -# from serial_port import SerialPort # use when running through the driver -# from resource_defs import locations, plate_definitions -# from resource_types import PlateResource +# from platecrane_driver.resource_defs import locations, plate_definitions +# from platecrane_driver.resource_types import PlateResource +# from platecrane_driver.serial_port import ( +# SerialPort, # use when running through WEI REST clients +# ) + +from serial_port import SerialPort # use when running through the driver +from resource_defs import locations, plate_definitions +from resource_types import PlateResource + """ # TODOs: @@ -93,6 +91,18 @@ def lock_joints(self): command = "limp FALSE\r\n" self.__serial_port.send_command(command) + def set_speed(self, speed: int): + """Sets the speed of the plate crane arm. + + Args: + speed (int): (units = % of full speed) Speed at which to move the PlateCrane EX. Appies to all axes + + Returns: + None + """ + command = "SPEED " + str(speed) + self.__serial_port.send_command(command) + def get_location_list(self): """Displays all location information stored in the Plate Crane EX robot's memory""" From 038c2fb9163537f2917a8ddcda301bc19204cc4e Mon Sep 17 00:00:00 2001 From: caseystone Date: Wed, 31 Jul 2024 17:15:51 -0500 Subject: [PATCH 16/19] Set speed works --- src/platecrane_driver/platecrane_driver.py | 109 ++++++++++++--------- src/platecrane_driver/resource_defs.py | 107 +++++++++++++++++--- src/platecrane_driver/serial_port.py | 53 ++++++---- src/platecrane_driver/test.py | 49 ++++++++- src/platecrane_rest_node.py | 28 ++++++ 5 files changed, 260 insertions(+), 86 deletions(-) diff --git a/src/platecrane_driver/platecrane_driver.py b/src/platecrane_driver/platecrane_driver.py index 8bda731..e9224d2 100644 --- a/src/platecrane_driver/platecrane_driver.py +++ b/src/platecrane_driver/platecrane_driver.py @@ -3,15 +3,15 @@ import re import time -# from platecrane_driver.resource_defs import locations, plate_definitions -# from platecrane_driver.resource_types import PlateResource -# from platecrane_driver.serial_port import ( -# SerialPort, # use when running through WEI REST clients -# ) +from platecrane_driver.resource_defs import locations, plate_definitions +from platecrane_driver.resource_types import PlateResource +from platecrane_driver.serial_port import ( + SerialPort, # use when running through WEI REST clients +) -from serial_port import SerialPort # use when running through the driver -from resource_defs import locations, plate_definitions -from resource_types import PlateResource +# from serial_port import SerialPort # use when running through the driver +# from resource_defs import locations, plate_definitions +# from resource_types import PlateResource """ @@ -62,7 +62,7 @@ def initialize(self): self.home() self.platecrane_current_position = self.get_position() - def home(self, timeout=28): + def home(self): """Homes all of the axes. Returns to neutral position (above exchange) Args: @@ -74,7 +74,7 @@ def home(self, timeout=28): # Moves axes to home position command = "HOME\r\n" - self.__serial_port.send_command(command, timeout) + self.__serial_port.send_command(command, timeout=60) def get_status(self): """Checks status of plate_crane""" @@ -91,17 +91,19 @@ def lock_joints(self): command = "limp FALSE\r\n" self.__serial_port.send_command(command) - def set_speed(self, speed: int): - """Sets the speed of the plate crane arm. - - Args: + def set_speed(self, speed: int): + """Sets the speed of the plate crane arm. + + Args: speed (int): (units = % of full speed) Speed at which to move the PlateCrane EX. Appies to all axes - - Returns: + + Returns: None """ command = "SPEED " + str(speed) - self.__serial_port.send_command(command) + self.__serial_port.send_command(command, timeout=0, delay=1) + self.get_position() + print(f"SPEED SET TO {speed}%") def get_location_list(self): """Displays all location information stored in the Plate Crane EX robot's memory""" @@ -166,18 +168,14 @@ def get_position(self) -> list: try: # collect coordinates of current position - current_position = list( - self.__serial_port.send_command(command, 1).split(" ") - ) + current_position = list(self.__serial_port.send_command(command).split(" ")) print(current_position) current_position = [eval(x.strip(",")) for x in current_position] print(current_position) except Exception: # Fall back: overlapping serial responses were detected. Wait 5 seconds then resend latest command time.sleep(5) - current_position = list( - self.__serial_port.send_command(command, 1).split(" ") - ) + current_position = list(self.__serial_port.send_command(command).split(" ")) current_position = [eval(x.strip(",")) for x in current_position] return current_position @@ -263,7 +261,7 @@ def jog(self, axis, distance) -> None: """ command = "JOG %s,%d\r\n" % (axis, distance) - self.__serial_port.send_command(command, timeout=1.5) + self.__serial_port.send_command(command) def move_joint_angles(self, R: int, Z: int, P: int, Y: int) -> None: """Move to a specified location @@ -279,7 +277,7 @@ def move_joint_angles(self, R: int, Z: int, P: int, Y: int) -> None: command = "MOVE TEMP\r\n" try: - self.__serial_port.send_command(command, 2) + self.__serial_port.send_command(command, timeout=60) except Exception as err: print(err) @@ -290,14 +288,12 @@ def move_joint_angles(self, R: int, Z: int, P: int, Y: int) -> None: self.delete_location("TEMP") - def move_single_axis(self, axis: str, loc: str, delay_time=1.5) -> None: + def move_single_axis(self, axis: str, loc: str) -> None: """Moves on a single axis, using an existing location in PlateCrane EX device memory as reference Args: axis (str): axis to move along loc (str): name of location in PlateCrane EX device memory to use as reference - delay_time (float): serial command timeout. Seconds to wait before expecting a serial response - defaults to 1.5 seconds Raises: TODO @@ -316,16 +312,14 @@ def move_single_axis(self, axis: str, loc: str, delay_time=1.5) -> None: ) command = "MOVE_" + axis.upper() + " " + loc + "\r\n" - self.__serial_port.send_command(command, timeout=delay_time) + self.__serial_port.send_command(command) self.move_status = "COMPLETED" - def move_location(self, loc: str = None, move_time: float = 4.7) -> None: + def move_location(self, loc: str = None) -> None: """Moves all joint to the given location. Args: loc (str): location to move to - move_time (float): serial command timeout. Seconds to wait before expecting a serial response - defaults to 4.7 seconds Returns: None @@ -339,7 +333,7 @@ def move_location(self, loc: str = None, move_time: float = 4.7) -> None: ) cmd = "MOVE " + loc + "\r\n" - self.__serial_port.send_command(cmd, timeout=move_time) + self.__serial_port.send_command(cmd) def move_tower_neutral(self) -> None: """Moves the tower to neutral position @@ -371,7 +365,7 @@ def move_arm_neutral(self) -> None: def move_gripper_neutral(self) -> None: """Moves the gripper to neutral position""" - self.move_single_axis("P", "Safe", delay_time=0.3) + self.move_single_axis("P", "Safe") current_pos = self.get_position() self.move_joint_angles( @@ -404,7 +398,6 @@ def pick_plate_safe_approach( """ print("PICK PLATE SAFE APPROACH CALLED") - # open the gripper self.gripper_open() @@ -420,17 +413,16 @@ def pick_plate_safe_approach( # Rotate gripper current_pos = self.get_position() self.move_joint_angles( - R=current_pos[0], + R=current_pos[0], Z=current_pos[1], - P=locations[source].joint_angles[2], + P=locations[source].joint_angles[2], Y=current_pos[3], ) - # Lower z axis to safe_approach_z height current_pos = self.get_position() self.move_joint_angles( - R=current_pos[0], + R=current_pos[0], Z=locations[source].safe_approach_height, P=current_pos[2], Y=current_pos[3], @@ -441,7 +433,7 @@ def pick_plate_safe_approach( self.move_joint_angles( R=current_pos[0], Z=current_pos[1], - P=current_pos[2], + P=current_pos[2], Y=locations[source].joint_angles[3], ) @@ -504,12 +496,12 @@ def place_plate_safe_approach( Y=current_pos[3], ) - # Rotate gripper to correct orientation + # Rotate gripper to correct orientation current_pos = self.get_position() self.move_joint_angles( - R=current_pos[0], + R=current_pos[0], Z=current_pos[1], - P=locations[target].joint_angles[2], + P=locations[target].joint_angles[2], Y=current_pos[3], ) @@ -527,7 +519,7 @@ def place_plate_safe_approach( self.move_joint_angles( R=current_pos[0], Z=current_pos[1], - P=current_pos[2], + P=current_pos[2], Y=locations[target].joint_angles[3], ) @@ -607,14 +599,30 @@ def pick_plate_direct( # close the gripper self.gripper_close() - # tap arm on top of plate stack to determine stack height + # move the arm directly above the stack + self.move_joint_angles( + R=locations[source].joint_angles[0], + Z=current_pos[1], + P=locations[source].joint_angles[2], + Y=locations[source].joint_angles[3], + ) + + # decrease the plate crane speed + self.set_speed(50) + + # move down in z height to tap the top of the plates in stack self.move_joint_angles( R=locations[source].joint_angles[0], - Z=locations[source].joint_angles[1], + Z=locations[source].joint_angles[ + 1 + ], # this is the only axis that should need to move P=locations[source].joint_angles[2], Y=locations[source].joint_angles[3], ) + # set plate crane back to full speed + self.set_speed(100) + # Move up, open gripper, grab plate at correct height self.jog("Z", 1000) self.gripper_open() @@ -701,6 +709,10 @@ def place_plate_direct( Y=locations[target].joint_angles[3], ) + if target_type == "stack": + # lower plate crane speed + self.set_speed(50) + # Lower arm (z axis) to plate grip height current_pos = self.get_position() self.move_joint_angles( @@ -710,6 +722,11 @@ def place_plate_direct( Y=current_pos[3], ) + if target_type == "stack": + # return plate crane to sull speed + self.set_speed(100) + + # open gripper to release the plate self.gripper_open() self.move_tower_neutral() diff --git a/src/platecrane_driver/resource_defs.py b/src/platecrane_driver/resource_defs.py index f1d7655..25fd579 100644 --- a/src/platecrane_driver/resource_defs.py +++ b/src/platecrane_driver/resource_defs.py @@ -1,23 +1,100 @@ +"""Resource definitions for the platecrane in BIO 350.""" + # from resource_types import Location, PlateResource # through driver from platecrane_driver.resource_types import Location, PlateResource # through WEI # Locations accessible by the PlateCrane EX. [R (base), Z (vertical axis), P (gripper rotation), Y (arm extension)] locations = { - "Safe": Location(name="Safe", joint_angles=[182220, 2500, 460, -308], location_type="nest", safe_approach_height=0), - "Stack1": Location(name="Stack1", joint_angles=[164672, -32703, 472, 5389], location_type="stack", safe_approach_height=0), - "Stack2": Location(name="Stack2", joint_angles=[182220, -32703, 460, 5420], location_type="stack", safe_approach_height=0), - "Stack3": Location(name="Stack3", joint_angles=[199708, -32703, 514, 5484], location_type="stack", safe_approach_height=0), - "Stack4": Location(name="Stack4", joint_angles=[217401, -32703, 546, 5473], location_type="stack", safe_approach_height=0), - "Stack5": Location(name="Stack5", joint_angles=[235104, -32703, 532, 5453], location_type="stack", safe_approach_height=0), - "LidNest1": Location(name="LidNest1", joint_angles=[168355, -31800, 484, -306], location_type="nest", safe_approach_height=0), - "LidNest2": Location(name="LidNest2", joint_angles=[199805, -31800, 484, -306], location_type="nest", safe_approach_height=0), - "LidNest3": Location(name="LidNest3", joint_angles=[231449, -31800, 484, -306], location_type="nest", safe_approach_height=0), - "Solo.Position1": Location(name="Solo.Position1", joint_angles=[36703, -27951, -1000, 3630], location_type="nest", safe_approach_height=0), - "Solo.Position2": Location(name="Solo.Position2", joint_angles=[53182, -27951, -413, 834], location_type="nest", safe_approach_height=0), - "Hidex.Nest": Location(name="Hidex.Nest", joint_angles=[102327, -31090, -5840, 2389], location_type="nest", safe_approach_height=-27033), - "Sealer.Nest": Location(name="Sealer.Nest", joint_angles=[117412, 920, -4766, 4514], location_type="nest", safe_approach_height=0), - "Peeler.Nest": Location(name="Peeler.Nest", joint_angles=[292635, -31008, -4521, 4235], location_type="nest", safe_approach_height=0), - "Liconic.Nest": Location(name="Liconic.Nest", joint_angles=[265563, -19800, -5413, 4978], location_type="nest", safe_approach_height=0), + "Safe": Location( + name="Safe", + joint_angles=[182220, 2500, 460, -308], + location_type="nest", + safe_approach_height=0, + ), + "Stack1": Location( + name="Stack1", + joint_angles=[164672, -32703, 472, 5389], + location_type="stack", + safe_approach_height=0, + ), + "Stack2": Location( + name="Stack2", + joint_angles=[182220, -32703, 460, 5420], + location_type="stack", + safe_approach_height=0, + ), + "Stack3": Location( + name="Stack3", + joint_angles=[199708, -32703, 514, 5484], + location_type="stack", + safe_approach_height=0, + ), + "Stack4": Location( + name="Stack4", + joint_angles=[217401, -32703, 546, 5473], + location_type="stack", + safe_approach_height=0, + ), + "Stack5": Location( + name="Stack5", + joint_angles=[235104, -32703, 532, 5453], + location_type="stack", + safe_approach_height=0, + ), + "LidNest1": Location( + name="LidNest1", + joint_angles=[168355, -31800, 484, -306], + location_type="nest", + safe_approach_height=0, + ), + "LidNest2": Location( + name="LidNest2", + joint_angles=[199805, -31800, 484, -306], + location_type="nest", + safe_approach_height=0, + ), + "LidNest3": Location( + name="LidNest3", + joint_angles=[231449, -31800, 484, -306], + location_type="nest", + safe_approach_height=0, + ), + "Solo.Position1": Location( + name="Solo.Position1", + joint_angles=[36703, -27951, -1000, 3630], + location_type="nest", + safe_approach_height=0, + ), + "Solo.Position2": Location( + name="Solo.Position2", + joint_angles=[53182, -27951, -413, 834], + location_type="nest", + safe_approach_height=0, + ), + "Hidex.Nest": Location( + name="Hidex.Nest", + joint_angles=[102327, -31090, -5840, 2389], + location_type="nest", + safe_approach_height=-27033, + ), + "Sealer.Nest": Location( + name="Sealer.Nest", + joint_angles=[117412, 920, -4766, 4514], + location_type="nest", + safe_approach_height=0, + ), + "Peeler.Nest": Location( + name="Peeler.Nest", + joint_angles=[292635, -31008, -4521, 4235], + location_type="nest", + safe_approach_height=0, + ), + "Liconic.Nest": Location( + name="Liconic.Nest", + joint_angles=[265563, -19800, -5413, 4978], + location_type="nest", + safe_approach_height=0, + ), } # Dimensions of labware used on the BIO_Workcells diff --git a/src/platecrane_driver/serial_port.py b/src/platecrane_driver/serial_port.py index 30108ce..ef7f2ed 100644 --- a/src/platecrane_driver/serial_port.py +++ b/src/platecrane_driver/serial_port.py @@ -49,12 +49,15 @@ def __disconnect_robot(self): else: print("Robot is successfully disconnected") - def send_command(self, command, timeout=0): + def send_command(self, command, timeout=10, delay=0): """ Sends provided command to Peeler and stores data outputted by the peeler. Indicates when the confirmation that the Peeler received the command by displaying 'ACK TRUE.' """ + print_command = command.strip("\r\n") + print(f"Sending command '{print_command}'") + send_time = time.time() try: self.connection.write(command.encode("utf-8")) @@ -65,14 +68,18 @@ def send_command(self, command, timeout=0): response_msg = "" initial_command_msg = "" - time.sleep(timeout) + time.sleep(delay) - while initial_command_msg == "": - response_msg, initial_command_msg = self.receive_command(timeout) + response_msg, initial_command_msg = self.receive_command( + initial_command_msg=command.strip("\r\n"), timeout=timeout + ) # Print the full output message including the initial command that was sent - print(initial_command_msg) - print(response_msg) + print_command = initial_command_msg.strip("\r\n") + print_response = response_msg.strip("\r\n") + print( + f"Command '{print_command}': {print_response} (elapsed time: {time.time() - send_time} seconds)" + ) error_codes = { "21": "R axis error", @@ -89,7 +96,7 @@ def send_command(self, command, timeout=0): return response_msg - def receive_command(self, time_wait): + def receive_command(self, initial_command_msg="", timeout=0): """ Records the data outputted by the plate_crane and sets it to equal "" if no data is outputted in the provided time. """ @@ -97,16 +104,22 @@ def receive_command(self, time_wait): # response_string = self.connection.read_until(expected=b'\r').decode('utf-8') response = "" response_string = "" - initial_command_msg = "" - - if self.connection.in_waiting != 0: - response = self.connection.readlines() - initial_command_msg = response[0].decode("utf-8").strip("\r\n") - if len(response) > 1: - for line_index in range(1, len(response)): - response_string += "\n" + response[line_index].decode( - "utf-8" - ).strip("\r\n") - else: - response_string = "" - return response_string, initial_command_msg + response_command_msg = "" + + start_wait = time.time() + while True: + if self.connection.in_waiting != 0: + response = self.connection.readlines() + if response[0].decode("utf-8").strip("\r\n") == initial_command_msg: + response_command_msg = initial_command_msg + if len(response) > 1: + for line_index in range(1, len(response)): + response_string += "\n" + response[line_index].decode( + "utf-8" + ).strip("\r\n") + else: + response_string = response[0].decode("utf-8").strip("\r\n") + if time.time() - start_wait > timeout or response_string != "": + break + time.sleep(0.25) + return response_string, response_command_msg diff --git a/src/platecrane_driver/test.py b/src/platecrane_driver/test.py index 3abee01..1d8055f 100644 --- a/src/platecrane_driver/test.py +++ b/src/platecrane_driver/test.py @@ -16,18 +16,57 @@ target_loc = "HidexNest2" lidnest3 = "LidNest3" sealer = "SealerNest" - s.move_location("Safe") + # s.move_location("Safe") # print(s.lid_height) # s.home() - print(s.get_location_list()) + # s.home() + + # print(s.get_location_list()) + # s.set_location("Solo.Position2", 53182, -27797, -413, 834) + # print(s.get_location_list()) # s.set_location("Solo.Position2", 53182, -27797, -413, 834) # s.set_location("HidexSafe", 209959, -2500, 490, -262) + # s.transfer("Solo.Position1","Solo.Position2", plate_type="flat_bottom_96well") # works if don't specify plate type (picks up lower) + # s.transfer("Stack1","Solo.Position1", plate_type="flat_bottom_96well", has_lid=True) # works if don't specify plate type (picks up lower) + # s.transfer("Solo.Position1","Hidex.Nest", plate_type="flat_bottom_96well", height_offset = 8) # works if don't specify plate type (picks up lower) + + # s.remove_lid(source="Solo.Position1", target="LidNest1", plate_type="flat_bottom_96well") + # s.replace_lid(source="LidNest1", target="Solo.Position1", plate_type="flat_bottom_96well") + + # s.transfer("Hidex.Nest","Sealer.Nest", plate_type="flat_bottom_96well", height_offset = 8) # works if don't specify plate type (picks up lower) + # s.transfer("Solo.Position2", "Sealer.Nest", plate_type="flat_bottom_96well", has_lid=False) # works if don't specify plate type (picks up lower) + + # DEMO MOVEMENTS + # s.transfer("Stack1", "Solo.Position2", plate_type="flat_bottom_96well", has_lid=True) + # s.remove_lid("Solo.Position2", "LidNest1", plate_type="flat_bottom_96well") + # s.transfer("Solo.Position2", "Hidex.Nest", plate_type="flat_bottom_96well", has_lid=False, height_offset=8) + # s.transfer("Sealer.Nest", "Liconic.Nest", plate_type="flat_bottom_96well", has_lid=False) + # s.transfer("Stack1", "Solo.Position2", plate_type="flat_bottom_96well", has_lid=False) + # s.transfer("Solo.Position2", "Stack1", plate_type="flat_bottom_96well", has_lid=False) + + # s.set_speed(100) + s.transfer( + "Stack1", "Solo.Position2", plate_type="flat_bottom_96well", has_lid=False + ) + # s.transfer("Solo.Position2", "Stack1", plate_type="flat_bottom_96well", has_lid=False) + + # s.transfer("Peeler.Nest", "Hidex.Nest", plate_type="flat_bottom_96well", has_lid=False, height_offset =8) + + # s.transfer("Solo.Position2", "Liconic.Nest", plate_type="flat_bottom_96well", has_lid=False) + + # s.transfer("Peeler.Nest", "Solo.Position2", plate_type="flat_bottom_96well", has_lid=True) # works if don't specify plate type (picks up lower) + # s.transfer("Stack1","Stack2", plate_type="flat_bottom_96well", has_lid=True) # works if don't specify plate type (picks up lower) + # s.transfer("Solo.Position2","Solo.Position2",source_type="stack",target_type="stack", plate_type="96_well", height_offset=-200) # works if don't specify plate type (picks up lower) # s.transfer("Stack1","Solo.Position2",source_type="stack",target_type="stack", plate_type="96_well", height_offset=-250) # works if don't specify plate type (picks up lower) - # s.remove_lid("Solo.Position2", "LidNest1", plate_type="96_well", height_offset=-650) + # s.replace_lid("LidNest1", "Solo.Position2", plate_type="flat_bottom_96well") + + # s.transfer("Solo.Position2","Solo.Position1", source_type="stack", target_type="stack", plate_type="flat_bottom_96", height_offset=0) + # s.transfer("Stack1","Solo.Position2",source_type="stack",target_type="stack", plate_type="96_well") # doesn't work with plate type through driver (picks up higher) + # s.get_position() # s.transfer("Solo.Position2","Solo.Position1", source_type="stack", target_type="module", plate_type="96_well", height_offset=-250) # s.transfer("Stack1","Solo.Position2",source_type="stack",target_type="stack", plate_type="96_well") # doesn't work with plate type through driver (picks up higher) @@ -44,7 +83,7 @@ # s.move_location("Safe") # s.move_location("Stack4") - # s.move_single_axis("Z", "Safe", delay_time=1) # move all the way up in z height + # s.move_single_axis("Z", "Safe") # move all the way up in z height # print(s.get_location_list()) @@ -177,7 +216,7 @@ # s.get_location_list() # s.move_joints_neutral() -# s.move_single_axis("R", "Safe", delay_time=1) +# s.move_single_axis("R", "Safe") # s.set_location("Safe",R=195399,Z=0,P=0,Y=0) # s.set_location("LidNest2",R=131719,Z=-31001,P=-5890,Y=-315) # s.transfer(source="LidNest1",target="LidNest2",source_type="stack",target_type="stack", plate_type="96_well") diff --git a/src/platecrane_rest_node.py b/src/platecrane_rest_node.py index 5dbc9f1..08a80d0 100644 --- a/src/platecrane_rest_node.py +++ b/src/platecrane_rest_node.py @@ -169,6 +169,18 @@ async def about(): description="This action moves the arm to a safe location.", args=[], ), + ModuleAction( + name="set_speed", + description="This action sets the speed of the PlateCrane EX.", + args=[ + ModuleActionArg( + name="speed", + description="Speed at which to set the plate crane arm. (units = percentage of total speed)", + type="int", + required=True, + ), + ], + ), ], resource_pools=[], ) @@ -224,6 +236,8 @@ def do_action(action_handle: str, action_vars): print("Height Offset: " + str(height_offset)) has_lid = action_args.get("has_lid", False) print("Has Lid: " + str(bool(has_lid))) + speed = action_args.get("speed", 100) + print("Speed: " + str(int(speed))) if action_handle == "transfer": print("Starting the transfer request") @@ -302,6 +316,20 @@ def do_action(action_handle: str, action_vars): state = ModuleStatus.IDLE print("Finished Action: " + action_handle.upper()) return response + elif action_handle == "set_speed": + try: + platecrane.set_speed(speed=speed) + except Exception as err: + response.action_response = StepStatus.FAILED + response.action_log = "Set Speed Failed. Error:" + str(err) + print(str(err)) + state = ModuleStatus.ERROR + else: + response.action_response = StepStatus.SUCCEEDED + response.action_msg = "Set Speed completed" + state = ModuleStatus.IDLE + print("Finished Action: " + action_handle.upper()) + return response else: msg = "UNKNOWN ACTION REQUEST! Available actions: status, home, get_plate" response.action_response = StepStatus.FAILED From 1c56ceba48dfa9b40858615e975dfe833d5dcc5f Mon Sep 17 00:00:00 2001 From: caseystone Date: Thu, 1 Aug 2024 10:56:33 -0500 Subject: [PATCH 17/19] Update to latest rest module for platecrane --- Dockerfile | 2 +- pyproject.toml | 2 +- src/platecrane_rest_node.py | 438 +++++++++--------------------------- src/sciclops_rest_node.py | 21 +- 4 files changed, 121 insertions(+), 342 deletions(-) diff --git a/Dockerfile b/Dockerfile index 0e9afe2..7cf5dc7 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,4 +1,4 @@ -FROM ghcr.io/ad-sdl/wei:v0.5.8 +FROM ghcr.io/ad-sdl/wei:v0.5.9 LABEL org.opencontainers.image.source=https://github.com/AD-SDL/hudson_platecrane_module LABEL org.opencontainers.image.description="Drivers and REST API's for the Hudson Platecrane and Sciclops robots" diff --git a/pyproject.toml b/pyproject.toml index 152ffb0..526e3eb 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,6 +1,6 @@ [project] name = "hudson_platecrane_module" -version = "1.3.0" +version = "1.4.0" description = "Driver for the Platecrane and Sciclops" authors = [ {name = "Ryan D. Lewis", email="ryan.lewis@anl.gov"}, diff --git a/src/platecrane_rest_node.py b/src/platecrane_rest_node.py index 08a80d0..9420d6d 100644 --- a/src/platecrane_rest_node.py +++ b/src/platecrane_rest_node.py @@ -1,351 +1,129 @@ #! /usr/bin/env python3 """The server for the Hudson Platecrane/Sciclops that takes incoming WEI flow requests from the experiment application""" -import json -import traceback -from argparse import ArgumentParser, Namespace -from contextlib import asynccontextmanager from pathlib import Path +from typing import List, Union -from fastapi import FastAPI -from fastapi.responses import JSONResponse +from fastapi.datastructures import State from platecrane_driver.platecrane_driver import PlateCrane -from wei.core.data_classes import ( - ModuleAbout, - ModuleAction, - ModuleActionArg, - ModuleStatus, - StepResponse, - StepStatus, +from typing_extensions import Annotated +from wei.modules.rest_module import RESTModule +from wei.utils import extract_version + +rest_module = RESTModule( + name="platecrane_node", + version=extract_version(Path(__file__).parent.parent / "pyproject.toml"), + description="A node to control the Hudson PlateCrane robot", + model="Hudson PlateCrane EX", ) -from wei.helpers import extract_version -global platecrane, state - - -def parse_args() -> Namespace: - """Argument parser""" - parser = ArgumentParser() - parser.add_argument( - "--host", - type=str, - default="0.0.0.0", - help="Hostname that the REST API will be accessible on", +rest_module.arg_parser.add_argument("--device", type=str, default="/dev/ttyUSB0") + +rest_module.state.platecrane = None + + +@rest_module.startup() +def platecrane_startup(state: State): + """Handles initializing the platecrane driver.""" + state.platecrane = None + state.platecrane = PlateCrane(host_path=state.device) + print("PLATECRANE online") + + +@rest_module.action() +def transfer( + state: State, + source: Annotated[ + Union[List[float], str], "The workcell location to grab the plate from" + ], + target: Annotated[ + Union[List[float], str], "The workcell location to place the plate at" + ], + plate_type: Annotated[str, "The type of plate being manipulated"] = "96_well", + height_offset: Annotated[ + int, "Amount to adjust the vertical grip point on the plate, in mm" + ] = 0, + has_lid: Annotated[ + bool, "Whether or not the plate currently has a lid on it" + ] = False, +): + """This action picks up a plate from one location and transfers is to another.""" + platecrane: PlateCrane = state.platecrane + platecrane.transfer( + source, + target, + plate_type=plate_type, + height_offset=int(height_offset), + has_lid=has_lid, ) - parser.add_argument("--port", type=int, default=2002) - parser.add_argument("--device", type=str, default="/dev/ttyUSB0") - return parser.parse_args() - - -@asynccontextmanager -async def lifespan(app: FastAPI): - """Initial run function for the app, parses the workcell argument - Parameters - ---------- - app : FastApi - The REST API app being initialized - - Returns - ------- - None""" - global platecrane, state - - args = parse_args() - - try: - platecrane = PlateCrane(host_path=args.device) - - except Exception as error_msg: - traceback.print_exc() - state = "PLATECRANE CONNECTION ERROR" - print("------- PlateCrane Error message: " + str(error_msg) + (" -------")) - - else: - print("PLATECRANE online") - state = ModuleStatus.IDLE - yield - pass - - -app = FastAPI( - lifespan=lifespan, -) - -@app.get("/state") -def get_state(): - """Returns the current state of the platecrane""" - global platecrane, state - return JSONResponse(content={"State": state}) - -@app.get("/about") -async def about(): - """Returns a description of the actions and resources the module supports""" - global state - about = ModuleAbout( - name="Hudson Platecrane", - description="Platecrane is a robotic arm module that can pick up and move plates between locations.", - interface="wei_rest_node", - version=extract_version(Path(__file__).parent.parent / "pyproject.toml"), - actions=[ - ModuleAction( - name="transfer", - description="This action picks up a plate from one location and transfers it to another.", - args=[ - ModuleActionArg( - name="source", - description="The workcell location to grab the plate from", - type="List[float], str", - required=True, - ), - ModuleActionArg( - name="target", - description="The workcell location to put the plate at", - type="List[float], str", - required=True, - ), - ModuleActionArg( - name="plate_type", - description="Type of plate.", - type="str", - required=False, - default="96_well", - ), - ], - ), - ModuleAction( - name="remove_lid", - description="This action picks up a plate's lid from one location and places it at another.", - args=[ - ModuleActionArg( - name="source", - description="The workcell location to grab the lid from", - type="List[float], str", - required=True, - ), - ModuleActionArg( - name="target", - description="The workcell location to put the lid", - type="List[float], str", - required=True, - ), - ModuleActionArg( - name="plate_type", - description="Type of plate.", - type="str", - required=False, - default="96_well", - ), - ], - ), - ModuleAction( - name="replace_lid", - description="This action picks up an unattached plate lid and places it on a plate.", - args=[ - ModuleActionArg( - name="source", - description="The workcell location to grab the lid from", - type="List[float], str", - required=True, - ), - ModuleActionArg( - name="target", - description="The workcell location to put the lid", - type="List[float], str", - required=True, - ), - ModuleActionArg( - name="plate_type", - description="Type of plate.", - type="str", - required=False, - default="96_well", - ), - ], - ), - ModuleAction( - name="move_safe", - description="This action moves the arm to a safe location.", - args=[], - ), - ModuleAction( - name="set_speed", - description="This action sets the speed of the PlateCrane EX.", - args=[ - ModuleActionArg( - name="speed", - description="Speed at which to set the plate crane arm. (units = percentage of total speed)", - type="int", - required=True, - ), - ], - ), - ], - resource_pools=[], +@rest_module.action() +def remove_lid( + state: State, + source: Annotated[ + Union[List[float], str], "The workcell location to grab the lib from" + ], + target: Annotated[ + Union[List[float], str], "The workcell location to place the lid at" + ], + plate_type: Annotated[str, "The type of plate the lid is on"] = "96_well", + height_offset: Annotated[ + int, "Amount to adjust the vertical grip point on the plate, in mm" + ] = 0, +): + """This action picks up a plate lid from a plate and transfers is to another location.""" + platecrane: PlateCrane = state.platecrane + platecrane.remove_lid( + source=source, + target=target, + plate_type=plate_type, + height_offset=height_offset, ) - return JSONResponse(content=about.model_dump(mode="json")) - - -@app.get("/resources") -async def resources(): - """Returns the current resources available to the module""" - global platecrane - return JSONResponse(content={"State": platecrane.get_status()}) - - -@app.post("/action") -def do_action(action_handle: str, action_vars): - """ - Runs an action on the module - - Parameters - ---------- - action_handle : str - The name of the action to be performed - action_vars : str - Any arguments necessary to run that action. - This should be a JSON object encoded as a string. - - Returns - ------- - response: StepResponse - A response object containing the result of the action - """ - global state, platecrane - response = StepResponse() - if state == "PLATECRANE CONNECTION ERROR": - message = "Connection error, cannot accept a job!" - response.action_response = StepStatus.FAILED - response.action_log = message - return response - if state == ModuleStatus.ERROR: - return response - action_args = json.loads(action_vars) - state = ModuleStatus.BUSY - - source = action_args.get("source") - print("Source location: " + str(source)) - target = action_args.get("target") - print("Target location: " + str(target)) - plate_type = action_args.get("plate_type") - print("Plate type: " + str(plate_type)) - height_offset = action_args.get("height_offset", 0) - print("Height Offset: " + str(height_offset)) - has_lid = action_args.get("has_lid", False) - print("Has Lid: " + str(bool(has_lid))) - speed = action_args.get("speed", 100) - print("Speed: " + str(int(speed))) - - if action_handle == "transfer": - print("Starting the transfer request") +@rest_module.action() +def replace_lid( + state: State, + source: Annotated[ + Union[List[float], str], "The workcell location to grab the lib from" + ], + target: Annotated[ + Union[List[float], str], "The workcell location to place the lid at" + ], + plate_type: Annotated[str, "The type of plate the lid is on"] = "96_well", + height_offset: Annotated[ + int, "Amount to adjust the vertical grip point on the plate, in mm" + ] = 0, +): + """This action picks up a plate lid from a location and places it on a plate.""" + platecrane: PlateCrane = state.platecrane + platecrane.replace_lid( + source=source, + target=target, + plate_type=plate_type, + height_offset=height_offset, + ) - try: - platecrane.transfer( - source, - target, - plate_type=plate_type, - height_offset=int(height_offset), - has_lid=has_lid, - ) - except Exception as err: - response.action_response = StepStatus.FAILED - response.action_log = "Transfer failed. Error:" + str(err) - print(str(err)) - state = ModuleStatus.ERROR - else: - response.action_response = StepStatus.SUCCEEDED - response.action_msg = "Transfer successfully completed" - state = ModuleStatus.IDLE - print("Finished Action: " + action_handle.upper()) - return response - elif action_handle == "remove_lid": - try: - platecrane.remove_lid( - source=source, - target=target, - plate_type=plate_type, - height_offset=height_offset, - ) - except Exception as err: - response.action_response = StepStatus.FAILED - response.action_log = "Remove lid failed. Error:" + str(err) - print(str(err)) - state = ModuleStatus.ERROR - else: - response.action_response = StepStatus.SUCCEEDED - response.action_msg = "Remove lid successfully completed" - state = ModuleStatus.IDLE - print("Finished Action: " + action_handle.upper()) - return response +@rest_module.action() +def move_safe( + state: State, +): + """This action moves the arm to a safe location (the location named "Safe").""" + platecrane: PlateCrane = state.platecrane + platecrane.move_location("Safe") - elif action_handle == "replace_lid": - try: - platecrane.replace_lid( - source=source, - target=target, - plate_type=plate_type, - height_offset=height_offset, - ) - except Exception as err: - response.action_response = StepStatus.FAILED - response.action_log = "Replace lid failed. Error:" + str(err) - print(str(err)) - state = ModuleStatus.ERROR - else: - response.action_response = StepStatus.SUCCEEDED - response.action_msg = "Replace lid successfully completed" - state = ModuleStatus.IDLE - print("Finished Action: " + action_handle.upper()) - return response - elif action_handle == "move_safe": - try: - platecrane.move_location("Safe") - except Exception as err: - response.action_response = StepStatus.FAILED - response.action_log = "Move Safe Failed. Error:" + str(err) - print(str(err)) - state = ModuleStatus.ERROR - else: - response.action_response = StepStatus.SUCCEEDED - response.action_msg = "Move Safe successfully completed" - state = ModuleStatus.IDLE - print("Finished Action: " + action_handle.upper()) - return response - elif action_handle == "set_speed": - try: - platecrane.set_speed(speed=speed) - except Exception as err: - response.action_response = StepStatus.FAILED - response.action_log = "Set Speed Failed. Error:" + str(err) - print(str(err)) - state = ModuleStatus.ERROR - else: - response.action_response = StepStatus.SUCCEEDED - response.action_msg = "Set Speed completed" - state = ModuleStatus.IDLE - print("Finished Action: " + action_handle.upper()) - return response - else: - msg = "UNKNOWN ACTION REQUEST! Available actions: status, home, get_plate" - response.action_response = StepStatus.FAILED - response.action_log = msg - state = ModuleStatus.ERROR - return response +@rest_module.action() +def set_speed( + state: State, + speed: Annotated[int, "The speed at which the arm moves (as a percentage)."], +): + """This action sets the speed at which the plate crane arm moves (as a percentage)""" + platecrane: PlateCrane = state.platecrane + platecrane.set_speed(speed=speed) if __name__ == "__main__": - import uvicorn - - args = parse_args() - - uvicorn.run( - "platecrane_rest_node:app", - host=args.host, - port=args.port, - reload=False, - ) + rest_module.start() diff --git a/src/sciclops_rest_node.py b/src/sciclops_rest_node.py index e1a3abf..a59e02d 100644 --- a/src/sciclops_rest_node.py +++ b/src/sciclops_rest_node.py @@ -8,7 +8,7 @@ from typing_extensions import Annotated from wei.modules.rest_module import RESTModule from wei.types.module_types import ModuleStatus -from wei.types.step_types import ActionRequest, StepResponse, StepStatus +from wei.types.step_types import StepResponse from wei.utils import extract_version rest_module = RESTModule( @@ -43,28 +43,29 @@ def sciclops_startup(state: State): state.status = ModuleStatus.IDLE -@rest_module.action(name="status", description="force sciclops to check its status") -def status(state: State, action: ActionRequest): +@rest_module.action(name="status") +def status(state: State): + """Action that forces the sciclops to check its status.""" + sciclops: SCICLOPS = state.sciclops + sciclops.get_status() return StepResponse.step_succeeded(action_msg="Succesfully got status") - -@rest_module.action(name="home", description="force sciclops to check its status") -def home(state: State, action: ActionRequest): +@rest_module.action() +def home(state: State): + """Homes the sciclops""" state.sciclops.home() return StepResponse.step_succeeded(action_msg="Succesfully Homed Robot") - -@rest_module.action(name="get_plate", description="force sciclops to check its status") +@rest_module.action(name="get_plate") def get_plate( state: State, - action: ActionRequest, pos: Annotated[int, "Stack to get plate from"], lid: Annotated[bool, "Whether plate has a lid or not"] = False, trash: Annotated[bool, "Whether to use the trash"] = False, ): - print(state._state) + """Get a plate from a stack position and move it to transfer point (or trash)""" state.sciclops.get_plate(pos, lid, trash) return StepResponse.step_succeeded(action_msg="Succesfully got plate") From 65b14888a7db9e64f7246e27315043b04aa618c1 Mon Sep 17 00:00:00 2001 From: caseystone Date: Thu, 1 Aug 2024 12:49:24 -0500 Subject: [PATCH 18/19] Fix step returns --- src/platecrane_rest_node.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/platecrane_rest_node.py b/src/platecrane_rest_node.py index 9420d6d..0905208 100644 --- a/src/platecrane_rest_node.py +++ b/src/platecrane_rest_node.py @@ -8,6 +8,7 @@ from platecrane_driver.platecrane_driver import PlateCrane from typing_extensions import Annotated from wei.modules.rest_module import RESTModule +from wei.types.step_types import StepResponse from wei.utils import extract_version rest_module = RESTModule( @@ -46,7 +47,7 @@ def transfer( has_lid: Annotated[ bool, "Whether or not the plate currently has a lid on it" ] = False, -): +) -> StepResponse: """This action picks up a plate from one location and transfers is to another.""" platecrane: PlateCrane = state.platecrane platecrane.transfer( @@ -56,6 +57,7 @@ def transfer( height_offset=int(height_offset), has_lid=has_lid, ) + return StepResponse.step_succeeded("Transfer complete") @rest_module.action() @@ -80,6 +82,7 @@ def remove_lid( plate_type=plate_type, height_offset=height_offset, ) + return StepResponse.step_succeeded("Removed lid") @rest_module.action() @@ -104,6 +107,7 @@ def replace_lid( plate_type=plate_type, height_offset=height_offset, ) + return StepResponse.step_succeeded("Replaced lid") @rest_module.action() @@ -113,6 +117,7 @@ def move_safe( """This action moves the arm to a safe location (the location named "Safe").""" platecrane: PlateCrane = state.platecrane platecrane.move_location("Safe") + return StepResponse.step_succeeded("Moved to the Safe position") @rest_module.action() @@ -123,6 +128,7 @@ def set_speed( """This action sets the speed at which the plate crane arm moves (as a percentage)""" platecrane: PlateCrane = state.platecrane platecrane.set_speed(speed=speed) + return StepResponse.step_succeeded(f"Set speed to {speed}") if __name__ == "__main__": From 1de24d88c0151d55d03fa285bf70e620b3a0adf7 Mon Sep 17 00:00:00 2001 From: caseystone Date: Thu, 1 Aug 2024 12:52:12 -0500 Subject: [PATCH 19/19] Bump version --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 526e3eb..636d562 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,6 +1,6 @@ [project] name = "hudson_platecrane_module" -version = "1.4.0" +version = "1.4.1" description = "Driver for the Platecrane and Sciclops" authors = [ {name = "Ryan D. Lewis", email="ryan.lewis@anl.gov"},