diff --git a/definitions/ur_module.info.yaml b/definitions/ur_module.info.yaml index 677e2b1..ed0b32d 100644 --- a/definitions/ur_module.info.yaml +++ b/definitions/ur_module.info.yaml @@ -161,6 +161,12 @@ actions: argument_type: float required: false default: 0.6 + joint_angle_locations: + name: joint_angle_locations + description: Use joint angles for the location + argument_type: bool + required: false + default: true locations: target: name: target diff --git a/src/ur_interface/integrated_controller.py b/src/ur_interface/integrated_controller.py new file mode 100644 index 0000000..ee8ee79 --- /dev/null +++ b/src/ur_interface/integrated_controller.py @@ -0,0 +1,218 @@ +"""Controller for integrating UR with various attachments""" + +import logging +import traceback +from copy import deepcopy +from enum import Enum +from typing import Optional, Union + +from madsci.client.resource_client import ResourceClient +from madsci.common.types.auth_types import OwnershipInfo +from madsci.common.types.location_types import LocationArgument + +from ur_interface.ur_controller import URController +from ur_interface.ur_dashboard import URDashboard +from ur_interface.ur_tools.grippers.abstract_gripper_interfaces import Gripper +from ur_interface.ur_tools.grippers.robotiq_2_finger_gripper_interface import Robotiq2FingerGripper + + +class UREndEffector(str, Enum): + """Possible end effectors for the UR arm""" + + ROBOTIQ2FINGERGRIPPER = "ROBOTIQ2FINGERGRIPPER" + SCREWDRIVER = "SCREWDRIVER" + PIPETTE = "PIPETTE" + WMTOOLCHANGER = "WMTOOLCHANGER" + + +end_effectors = {UREndEffector.ROBOTIQ2FINGERGRIPPER: Robotiq2FingerGripper} + + +class IntegratedController: + """ + This is the primary class for UR robots. + It integrates various interfaces to achieve comprehensive control, encompassing robot initialization via the UR dashboard, + robot motion using URx, and the management of robot end-effectors such as grippers, screwdrivers, electronic pipettes, and cameras." + """ + + def __init__( + self, + hostname: str = None, + resource_client: ResourceClient = None, + end_effector: UREndEffector = None, + end_effector_resource_id: str = None, + resource_owner: OwnershipInfo = None, + tool_resource_id: str = None, + tcp_pose: list = [0, 0, 0, 0, 0, 0], + base_reference_frame: list = None, + logger: Optional[logging.Logger] = None, + ): + """Constructor for the UR class. + :param hostname: Hostname or ip. + :param logger: Logger object for logging messages + """ + + if not hostname: + raise TypeError("Hostname cannot be None Type!") + + self.hostname = hostname + self.resource_client = resource_client + self.tool_resource_id = tool_resource_id + self.resource_owner = resource_owner + self.logger = logger or self._setup_logger() + self.acceleration = 0.5 + self.velocity = 0.5 + self.robot_current_joint_angles = None + self.end_effector_resource_id = end_effector_resource_id + + try: + self.ur_dashboard = URDashboard(hostname=self.hostname) + self.ur_controller = URController( + hostname=self.hostname, logger=self.logger, tcp_pose=tcp_pose, base_reference_frame=base_reference_frame + ) + + self.ur_controller.ur_connection.set_tcp(tcp_pose) + self.create_end_effector_controller(end_effector) + except Exception as e: + self.logger.error(f"Failed to initialize UR: {e}\n{traceback.format_exc()}") + raise e + + def create_end_effector_controller(self, end_effector: Optional[UREndEffector]) -> None: + """Create appropriate end-effector controller.""" + self.logger.log_info("Creating Robotiq 2-finger gripper controller") + if end_effector is not None: + self.end_effector = end_effectors[end_effector](hostname=self.hostname) + if self.end_effector.tool_params: + self.ur_controller.ur_connection.set_tool_communication(**self.end_effector.tool_params) + + def gripper_pick( + self, + source: Union[LocationArgument, list], + home: Union[LocationArgument, list, None] = None, + source_approach_axis: str = None, + source_approach_distance: float = None, + ): + """Pick up from source position""" + if not isinstance(self.end_effector, Gripper): + raise Exception("End-effector is not a gripper, cannot perform pick operation") + if isinstance(source, LocationArgument): + source_location = source.representation["linear_coordinates"] + elif isinstance(source, list): + source_location = source + else: + raise Exception("Please provide an appropriate source location") + if home is not None: + if isinstance(home, LocationArgument): + home_location = home.representation["joint_angles"] + elif isinstance(home, list): + home_location = home + else: + raise Exception("Please provide an appropriate source location") + self.ur_controller.move_to_location(home_location) + + if not source_approach_distance: + source_approach_distance = 0.05 + + axis = None + + if not source_approach_axis or source_approach_axis.lower() == "z": + axis = 2 + elif source_approach_axis.lower() == "y": + axis = 1 + elif source_approach_axis.lower() == "-y": + axis = 1 + source_approach_distance = -source_approach_distance + elif source_approach_axis.lower() == "x": + axis = 0 + elif source_approach_axis.lower() == "-x": + axis = 0 + source_approach_distance = -source_approach_distance + + above_goal = deepcopy(source_location) + above_goal[axis] += source_approach_distance + + self.logger.info(f"Starting pick operation from source: {source_location}") + + self.end_effector.open() + + self.logger.debug("Moving to above goal position") + self.ur_controller.move_to_location(above_goal, linear_motion=True) + + self.logger.info("Moving to goal position") + self.ur_controller.move_to_location(source_location, linear_motion=True) + + self.end_effector.close() + + self.logger.debug("Moving back to above goal position") + self.ur_controller.move_to_location(above_goal, linear_motion=True) + self.logger.info("Pick operation completed successfully") + if self.resource_client is not None: + object, _ = self.resource_client.pop(source.resource_id) + self.resource_client.push(self.end_effector_resource_id, object) + + def gripper_place( + self, + target: Union[LocationArgument, list], + home: Union[LocationArgument, list, None] = None, + target_approach_axis: str = None, + target_approach_distance: float = None, + ): + """Pick up from source position""" + if not isinstance(self.end_effector, Gripper): + raise Exception("End-effector is not a gripper, cannot perform place operation") + if isinstance(target, LocationArgument): + target_location = target.representation["linear_coordinates"] + elif isinstance(target, list): + target_location = target + else: + raise Exception("Please provide an appropriate source location") + if home is not None: + if isinstance(home, LocationArgument): + home = home.representation["joint_angles"] + elif isinstance(home, list): + home = home + else: + raise Exception("Please provide an appropriate source location") + self.ur_controller.move_to_location(home) + + if not target_approach_distance: + target_approach_distance = 0.05 + + axis = None + + if not target_approach_axis or target_approach_axis.lower() == "z": + axis = 2 + elif target_approach_axis.lower() == "y": + axis = 1 + elif target_approach_axis.lower() == "-y": + axis = 1 + target_approach_distance = -target_approach_distance + elif target_approach_axis.lower() == "x": + axis = 0 + elif target_approach_axis.lower() == "-x": + axis = 0 + target_approach_distance = -target_approach_distance + + above_goal = deepcopy(target_location) + above_goal[axis] += target_approach_distance + + self.logger.info(f"Starting pick operation from source: {target_location}") + + self.logger.debug("Moving to above goal position") + self.ur_controller.move_to_location(above_goal, linear_motion=True) + + self.logger.debug("Moving to goal position") + self.ur_controller.move_to_location(target_location, linear_motion=True) + + self.end_effector.open() + if self.resource_client is not None: + object, _ = self.resource_client.pop(target_location.resource_id) + self.resource_client.push(self.end_effector_resource_id, object) + + self.logger.debug("Moving back to above goal position") + self.ur_controller.move_to_location(above_goal, linear_motion=True) + self.logger.info("Pick operation completed successfully") + + def disconnect(self) -> None: + """disconnect arm""" + self.ur_controller.disconnect() diff --git a/src/ur_interface/ur.py b/src/ur_interface/ur.py deleted file mode 100644 index ba27b54..0000000 --- a/src/ur_interface/ur.py +++ /dev/null @@ -1,989 +0,0 @@ -#!/usr/bin/env python3 -"""Interface for UR Driver""" - -import logging -import socket -import traceback -from math import radians -from time import sleep -from typing import Optional, Union - -import math3d as m3 -import numpy as np -from madsci.client.resource_client import ResourceClient -from madsci.common.types.auth_types import OwnershipInfo -from madsci.common.types.location_types import LocationArgument -from urx import Robot - -from ur_interface.ur_dashboard import UR_DASHBOARD -from ur_interface.ur_error_types import GripperError, URConnectionError, URMovementError -from ur_interface.ur_tools.gripper_controller import FingerGripperController -from ur_interface.ur_tools.ot_pipette_controller import OTPipetteController -from ur_interface.ur_tools.screwdriver_controller import ScrewdriverController -from ur_interface.ur_tools.tricontinent_pipette_controller import TricontinentPipetteController -from ur_interface.ur_tools.wm_tool_changer_controller import WMToolChangerController - - -class Connection: - """Connection to the UR robot to be shared within UR driver""" - - def __init__(self, hostname: str = "146.137.240.38", logger: logging.Logger = None) -> None: - """Connection class that creates a connection with the robot using URx Library""" - self.hostname = hostname - self.logger = logger - self.connection = None - self.connect_ur() - - def connect_ur(self): - """Create connection to the UR robot""" - for attempt in range(10): - try: - self.logger.info(f"Attempting robot connection (attempt {attempt + 1}/10)...") - self.connection = Robot(self.hostname) - - except socket.error as e: - self.logger.warning(f"Robot connection attempt {attempt + 1} failed: {e}") - sleep(1) - - except Exception as e: - self.logger.error(f"Unexpected error during robot connection: {e}\n{traceback.format_exc()}") - sleep(1) - - else: - self.logger.info("Successful UR connection") - return - - raise URConnectionError(f"Failed to connect to UR robot at {self.hostname} after 10 attempts") - - def disconnect_ur(self): - """ - Description: Disconnects the socket connection with the UR robot - """ - try: - if self.connection: - self.connection.close() - self.logger.info("Robot connection closed successfully") - except Exception as e: - self.logger.error(f"Error closing robot connection: {e}\n{traceback.format_exc()}") - - -class UR: - """ - This is the primary class for UR robots. - It integrates various interfaces to achieve comprehensive control, encompassing robot initialization via the UR dashboard, - robot motion using URx, and the management of robot end-effectors such as grippers, screwdrivers, electronic pipettes, and cameras." - """ - - def __init__( - self, - hostname: str = None, - resource_client: ResourceClient = None, - resource_owner: OwnershipInfo = None, - tool_resource_id: str = None, - tcp_pose: list = [0, 0, 0, 0, 0, 0], - base_reference_frame: list = None, - logger: Optional[logging.Logger] = None, - ): - """Constructor for the UR class. - :param hostname: Hostname or ip. - :param logger: Logger object for logging messages - """ - - if not hostname: - raise TypeError("Hostname cannot be None Type!") - - self.hostname = hostname - self.resource_client = resource_client - self.tool_resource_id = tool_resource_id - self.resource_owner = resource_owner - self.logger = logger or self._setup_logger() - - self.acceleration = 0.5 - self.velocity = 0.5 - self.robot_current_joint_angles = None - self.gripper_speed: int = None - self.gripper_force: int = None - - try: - self.ur_dashboard = UR_DASHBOARD(hostname=self.hostname) - self.ur = Connection(hostname=self.hostname, logger=self.logger) - self.ur_connection = self.ur.connection - - self.gripper_speed = 255 - self.gripper_force = 255 - - self.ur_connection.set_tcp(tcp_pose) - if base_reference_frame: - self._set_base_reference_frame(base_reference_frame) - self.get_movement_state() - - except Exception as e: - self.logger.error(f"Failed to initialize UR: {e}\n{traceback.format_exc()}") - raise - - def _setup_logger(self) -> logging.Logger: - """Setup default logger if none provided""" - logger = logging.getLogger(__name__) - logger.setLevel(logging.INFO) - if not logger.handlers: - handler = logging.StreamHandler() - formatter = logging.Formatter("%(asctime)s - %(name)s - %(levelname)s - %(message)s") - handler.setFormatter(formatter) - logger.addHandler(handler) - return logger - - def disconnect(self): - """Disconnects the robot from URX and UR Dashboard connections""" - try: - self.ur.disconnect_ur() - self.ur_dashboard.clear_operational_mode() - self.ur_dashboard.disconnect() - self.logger.info("UR disconnected successfully") - except Exception as e: - self.logger.error(f"Error during UR disconnect: {e}\n{traceback.format_exc()}") - - def _set_base_reference_frame(self, base_reference_frame: list) -> None: - """Sets the base reference frame for the robot. - Args: - base_reference_frame (list): 6 element reference frame. [x, y, z, rx, ry, rz]. Rotation values are in degrees, later converted to radians in the function. - """ - if not isinstance(base_reference_frame, list) or len(base_reference_frame) != 6: - raise ValueError("Base reference frame must be a list of 6 values") - - try: - # Extract position and rotation components - x, y, z, rx_deg, ry_deg, rz_deg = base_reference_frame - - # Create translation vector (only if any translation values are non-zero) - if any([x, y, z]): - translation = m3.Vector(x, y, z) - else: - translation = m3.Vector(0, 0, 0) - - # Start with identity rotation - rotation = m3.Orientation() # Identity rotation - - # Apply only non-zero rotations in order - if rx_deg != 0: - rx_rad = radians(rx_deg) - rotation = rotation * m3.Orientation.new_rot_x(rx_rad) - - if ry_deg != 0: - ry_rad = radians(ry_deg) - rotation = rotation * m3.Orientation.new_rot_y(ry_rad) - - if rz_deg != 0: - rz_rad = radians(rz_deg) - rotation = rotation * m3.Orientation.new_rot_z(rz_rad) - # Create the transform - transform = m3.Transform(rotation, translation) - - # Set the coordinate system - self.ur_connection.set_csys(transform) - - self.logger.info(f"Base reference frame set to: {base_reference_frame}") - except Exception as e: - self.logger.error(f"Error setting base reference frame: {e}\n{traceback.format_exc()}") - raise - - def get_movement_state(self) -> str: - """Gets robot movement status by checking robot joint values. - Return (str) READY if robot is not moving - BUSY if robot is moving - """ - try: - current_location = self.ur_connection.getj() - if self.robot_current_joint_angles is None: - movement_state = "READY" - else: - if np.linalg.norm(np.array(current_location) - np.array(self.robot_current_joint_angles)) < 1e-3: - movement_state = "READY" - else: - movement_state = "BUSY" - - self.robot_current_joint_angles = current_location - - return movement_state, current_location - - except Exception as e: - self.logger.error(f"Error getting movement state: {e}\n{traceback.format_exc()}") - raise URMovementError("Failed to get robot movement state") # noqa - - def home(self, home_location: Union[LocationArgument, list], linear_motion: bool = False) -> None: - """Moves the robot to the home location. - - Args: home_location: 6 joint value location - """ - try: - self.logger.info("Homing the robot...") - if isinstance(home_location, LocationArgument): - home_loc = home_location.location - else: - home_loc = home_location - if linear_motion: - self.ur_connection.movel(home_loc, self.velocity, self.acceleration) - else: - self.ur_connection.movej(home_loc, self.velocity, self.acceleration) - self.logger.info("Robot homed") - except Exception as e: - self.logger.error(f"Error in homing the robot: {e}\n{traceback.format_exc()}") - raise URMovementError("Failed to home the robot") # noqa - - def pick_tool( - self, - home: Union[LocationArgument, list] = None, - tool_loc: Union[LocationArgument, list] = None, - docking_axis: str = "y", - payload: float = 0.12, - tool_name: str = None, - ) -> None: - """Picks up a tool using the given tool location - Args - home (Union[LocationArgument, list]): Home location - tool_loc (Union[LocationArgument, list]): Tool location - docking_axis (str): Docking axis (x/y/z). Default: Y AXIS - payload (float): Weight of the end effector - tool_name (str): Name of the tool to indentify system variables - """ - - try: - self.ur_connection.set_payload(payload) - wingman_tool = WMToolChangerController( - tool_location=tool_loc, - docking_axis=docking_axis, - ur=self.ur_connection, - tool=tool_name, - resource_client=self.resource_client, - tool_resource_id=self.tool_resource_id, - ) - - self.home(home) - wingman_tool.pick_tool() - if self.resource_client and isinstance(tool_loc, LocationArgument): # Handle resources if configured - tool_resource = self.resource_client.get_resource(tool_loc.resource_id) - tool_resource.owner = self.resource_owner - self.resource_client.update_resource( - resource_id=tool_resource.resource_id, - ) - self.tool_resource_id = tool_resource.resource_id - self.home(home) - - except Exception as err: - self.logger.error(f"Error in picking tool: {err}\n{traceback.format_exc()}") - - def place_tool( - self, - home: Union[LocationArgument, list] = None, - tool_loc: Union[LocationArgument, list] = None, - docking_axis: str = "y", - tool_name: str = None, - ) -> None: - """Places a tool back to tool docking location - Args - home (Union[LocationArgument, list]): Home location - tool_loc (Union[LocationArgument, list]): Tool location - docking_axis (str): Docking axis (x/y/z). Default: Y AXIS - tool_name (str): Name of the tool to indentify system variables - - """ - try: - wingman_tool = WMToolChangerController( - tool_location=tool_loc, - docking_axis=docking_axis, - ur=self.ur_connection, - tool=tool_name, - resource_client=self.resource_client, - tool_resource_id=self.tool_resource_id, - ) - self.home(home) - wingman_tool.place_tool() - if self.resource_client and isinstance(tool_loc, LocationArgument): # Handle resources if configured - tool_resource = self.resource_client.get_resource(tool_loc.resource_id) - tool_resource.owner = None - self.resource_client.update_resource( - resource_id=tool_resource.resource_id, - ) - self.tool_resource_id = None - self.home(home) - - except Exception as err: - self.logger.error(f"Error in placing tool: {err}\n{traceback.format_exc()}") - - def set_digital_io(self, channel: int = None, value: bool = None) -> None: - """Sets digital I/O outputs to open an close the channel. This helps controlling the external tools - - Args - channel (int): Channel number - value (bool): False for close, True for open - """ - if channel is None or value is None: - self.logger.error("Channel or value is not specified") - return - self.ur_connection.set_digital_out(channel, value) - - def gripper_transfer( - self, - home: Union[LocationArgument, list] = None, - source: Union[LocationArgument, list] = None, - target: Union[LocationArgument, list] = None, - source_approach_axis: str = None, - target_approach_axis: str = None, - source_approach_distance: float = None, - target_approach_distance: float = None, - gripper_open: int = None, - gripper_close: int = None, - ) -> None: - """Make a transfer using the finger gripper. This function uses linear motions to perform the pick and place movements. - - Args - home (Union[LocationArgument, list]): Home location - source (Union[LocationArgument, list]): Source location - target(Union[LocationArgument, list]): Target location - source_approach_axis (str): Source approach axis (X/Y/Z) - target_approach_axis (str): Target approach axis (X/Y/Z) - source_approach_distance (float): Source approach distance. Unit meters. - target_approach_distance(float): Target approach distance. Unit meters. - gripper_open (int): Gripper max open value (0-255) - gripper_close (int): Gripper min close value (0-255) - - """ - - if not source or not target: - raise ValueError("Please provide both the source and target locations to make a transfer") - gripper_controller = None - - try: - self.logger.info(f"Starting gripper transfer from {source} to {target}") - self.home(home) - - self.logger.info("Initializing gripper controller") - gripper_controller = FingerGripperController( - hostname=self.hostname, - ur=self.ur_connection, - resource_client=self.resource_client, - gripper_resource_id=self.tool_resource_id, - logger=self.logger, - ) - self.logger.info("Connecting to gripper...") - gripper_controller.connect_gripper() - gripper_controller.velocity = self.velocity - gripper_controller.acceleration = self.acceleration - gripper_controller.gripper_speed = self.gripper_speed - gripper_controller.gripper_force = self.gripper_force - - if gripper_open: - gripper_controller.gripper_open = gripper_open - if gripper_close: - gripper_controller.gripper_close = gripper_close - - self.logger.info("Executing gripper transfer...") - gripper_controller.transfer( - home=home, - source=source, - target=target, - source_approach_axis=source_approach_axis, - target_approach_axis=target_approach_axis, - source_approach_distance=source_approach_distance, - target_approach_distance=target_approach_distance, - ) - self.logger.info("Gripper transfer completed successfully") - except socket.timeout as e: - self.logger.error(f"Socket timeout during gripper transfer: {e}\n{traceback.format_exc()}") - raise GripperError(f"Gripper communication timed out: {e}") # noqa - - except Exception as e: - self.logger.error(f"Error in gripper transfer action: {e}\n{traceback.format_exc()}") - raise GripperError(f"Gripper transfer failed: {e}") # noqa - - finally: - if gripper_controller: - try: - self.logger.info("Disconnecting gripper...") - gripper_controller.disconnect_gripper() - except Exception as e: - self.logger.error(f"Error disconnecting gripper: {e}\n{traceback.format_exc()}") - - try: - self.home(home) - except Exception as e: - self.logger.error(f"Error returning to home after gripper transfer: {e}\n{traceback.format_exc()}") - - def gripper_pick( - self, - home: Union[LocationArgument, list] = None, - source: Union[LocationArgument, list] = None, - source_approach_axis: str = None, - source_approach_distance: float = None, - gripper_close: int = None, - ) -> None: - """Make a transfer using the finger gripper. This function uses linear motions to perform the pick and place movements. - - Args - home (Union[LocationArgument, list]): Home location - source (Union[LocationArgument, list]): Source location - source_approach_axis (str): Source approach axis (X/Y/Z) - source_approach_distance (float): Source approach distance. Unit meters. - gripper_close (int): Gripper min close value (0-255) - - """ - if not source: - raise ValueError("Please provide the source location to make a pick") - - gripper_controller = None - - try: - self.logger.info(f"Starting gripper pick from {source}") - self.home(home) - - self.logger.info("Initializing gripper controller...") - gripper_controller = FingerGripperController( - hostname=self.hostname, - ur=self.ur_connection, - resource_client=self.resource_client, - gripper_resource_id=self.tool_resource_id, - logger=self.logger, - ) - - self.logger.info("Connecting to gripper...") - gripper_controller.connect_gripper() - gripper_controller.velocity = self.velocity - gripper_controller.acceleration = self.acceleration - gripper_controller.gripper_speed = self.gripper_speed - gripper_controller.gripper_force = self.gripper_force - - if gripper_close: - gripper_controller.gripper_close = gripper_close - - self.logger.info("Executing gripper pick...") - gripper_controller.pick( - source=source, - approach_axis=source_approach_axis, - approach_distance=source_approach_distance, - ) - self.logger.info("Gripper pick completed successfully") - - except socket.timeout as e: - self.logger.error(f"Socket timeout during gripper pick: {e}\n{traceback.format_exc()}") - raise GripperError(f"Gripper communication timed out during pick: {e}") # noqa - - except Exception as e: - self.logger.error(f"Error in gripper pick action: {e}\n{traceback.format_exc()}") - raise GripperError(f"Gripper pick failed: {e}") # noqa - - finally: - if gripper_controller: - try: - self.logger.info("Disconnecting gripper...") - gripper_controller.disconnect_gripper() - except Exception as e: - self.logger.error(f"Error disconnecting gripper: {e}\n{traceback.format_exc()}") - - try: - self.home(home) - except Exception as e: - self.logger.error(f"Error returning to home after gripper pick: {e}\n{traceback.format_exc()}") - - def gripper_place( - self, - home: Union[LocationArgument, list] = None, - target: Union[LocationArgument, list] = None, - target_approach_axis: str = None, - target_approach_distance: float = None, - gripper_open: int = None, - ) -> None: - """Make a transfer using the finger gripper. This function uses linear motions to perform the pick and place movements. - - Args - home (Union[LocationArgument, list]): Home location - target (Union[LocationArgument, list]): Source location - target_approach_axis (str): Source approach axis (X/Y/Z) - target_approach_distance (float): Source approach distance. Unit meters. - gripper_open (int): Gripper max open value (0-255) - - """ - - if not target: - raise ValueError("Please provide the target location to make a place") - - gripper_controller = None - - try: - self.logger.info(f"Starting gripper place to {target}") - self.home(home) - - gripper_controller = FingerGripperController( - hostname=self.hostname, - ur=self.ur_connection, - resource_client=self.resource_client, - gripper_resource_id=self.tool_resource_id, - logger=self.logger, - ) - - self.logger.info("Connecting to gripper...") - gripper_controller.connect_gripper() - gripper_controller.velocity = self.velocity - gripper_controller.acceleration = self.acceleration - gripper_controller.gripper_speed = self.gripper_speed - gripper_controller.gripper_force = self.gripper_force - - if gripper_open: - gripper_controller.gripper_open = gripper_open - - self.logger.info("Executing gripper place...") - gripper_controller.place( - target=target, - approach_axis=target_approach_axis, - approach_distance=target_approach_distance, - ) - self.logger.info("Gripper place completed successfully") - - except socket.timeout as e: - self.logger.error(f"Socket timeout during gripper place: {e}\n{traceback.format_exc()}") - raise GripperError(f"Gripper communication timed out during place: {e}") # noqa - - except Exception as e: - self.logger.error(f"Error in gripper place action: {e}\n{traceback.format_exc()}") - raise GripperError(f"Gripper place failed: {e}") # noqa - - finally: - if gripper_controller: - try: - self.logger.info("Disconnecting gripper...") - gripper_controller.disconnect_gripper() - except Exception as e: - self.logger.error(f"Error disconnecting gripper: {e}\n{traceback.format_exc()}") - - try: - self.home(home) - except Exception as e: - self.logger.error(f"Error returning to home after gripper place: {e}\n{traceback.format_exc()}") - - def gripper_screw_transfer( - self, - home: Union[LocationArgument, list] = None, - target: Union[LocationArgument, list] = None, - screwdriver_loc: Union[LocationArgument, list] = None, - screw_loc: Union[LocationArgument, list] = None, - screw_time: float = 9, - gripper_open: int = None, - gripper_close: int = None, - ) -> None: - """Using custom made screwdriving solution. This function uses linear motions to perform the pick and place movements. - - Args - home (Union[LocationArgument, list]): Home location - target(Union[LocationArgument, list]): Target location - screwdriver_loc (Union[LocationArgument, list]): Location of the screwdriving bit - screw_loc (Union[LocationArgument, list]): Location where the screwdriving will be performed - screw_time (float): Screwdriving duration - gripper_open (int): Gripper max open value (0-255) - gripper_close (int): Gripper min close value (0-255) - - """ - - self.home(home) - - try: - gripper_controller = FingerGripperController( - hostname=self.hostname, - ur=self.ur_connection, - resource_client=self.resource_client, - gripper_resource_id=self.tool_resource_id, - ) - - gripper_controller.connect_gripper() - - if gripper_open: - gripper_controller.gripper_open = gripper_open - if gripper_close: - gripper_controller.gripper_close = gripper_close - - gripper_controller.screw_transfer( - home=home, target=target, screw_loc=screw_loc, screwdriver_loc=screwdriver_loc, screw_time=screw_time - ) - - except Exception as err: - self.logger.error( - f"Error during gripper screw transfer: {err}\n{traceback.format_exc()}" - ) # Added exc_info=True for detailed logging - - finally: - gripper_controller.disconnect_gripper() - - def remove_cap( - self, - home: Union[LocationArgument, list] = None, - source: Union[LocationArgument, list] = None, - target: Union[LocationArgument, list] = None, - gripper_open: int = None, - gripper_close: int = None, - ) -> None: - """Remove vial cap. This function uses linear motions to perform the pick and place movements. - - Args - home (Union[LocationArgument, list]): Home location - source (Union[LocationArgument, list]): Source location - target (Union[LocationArgument, list]): Target location - gripper_open (int): Gripper max open value (0-255) - gripper_close (int): Gripper min close value (0-255) - - """ - self.home(home) - - try: - gripper_controller = FingerGripperController( - hostname=self.hostname, - ur=self.ur_connection, - resource_client=self.resource_client, - gripper_resource_id=self.tool_resource_id, - ) - - gripper_controller.connect_gripper() - if gripper_open: - gripper_controller.gripper_open = gripper_open - if gripper_close: - gripper_controller.gripper_close = gripper_close - - gripper_controller.remove_cap(home=home, target=target, source=source) - gripper_controller.disconnect_gripper() - - except Exception as err: - self.logger.error(f"{err}\n{traceback.format_exc()}") - - def place_cap( - self, - home: Union[LocationArgument, list] = None, - source: Union[LocationArgument, list] = None, - target: Union[LocationArgument, list] = None, - gripper_open: int = None, - gripper_close: int = None, - ) -> None: - """Place vial cap. This function uses linear motions to perform the pick and place movements. - - Args - home (Union[LocationArgument, list]): Home location - source (Union[LocationArgument, list]): Source location - target (Union[LocationArgument, list]): Target location - gripper_open (int): Gripper max open value (0-255) - gripper_close (int): Gripper min close value (0-255) - - """ - self.home(home) - - try: - gripper_controller = FingerGripperController( - hostname=self.hostname, - ur=self.ur_connection, - resource_client=self.resource_client, - gripper_resource_id=self.tool_resource_id, - ) - gripper_controller.place_cap(home=home, target=target, source=source) - gripper_controller.disconnect_gripper() - - except Exception as err: - self.logger.error(f"{err}\n{traceback.format_exc()}") - - def pick_and_flip_object( - self, - home: Union[LocationArgument, list] = None, - target: Union[LocationArgument, list] = None, - approach_axis: str = None, - gripper_open: int = None, - gripper_close: int = None, - ) -> None: - """ - Pick an object then flips it and puts it back to the same location. This function uses linear motions to perform the pick and place movements. - - Args - home (Union[LocationArgument, list]): Home location - target (Union[LocationArgument, list]): Target location - approach_axis (str) = Object approach axis - gripper_open (int): Gripper max open value (0-255) - gripper_close (int): Gripper min close value (0-255) - - """ - - self.home(home) - - try: - gripper_controller = FingerGripperController( - hostname=self.hostname, - ur=self.ur_connection, - resource_client=self.resource_client, - gripper_resource_id=self.gripper_resource_id, - ) - - gripper_controller.connect_gripper() - - if gripper_open: - gripper_controller.gripper_open = gripper_open - if gripper_close: - gripper_controller.gripper_close = gripper_close - - gripper_controller.flip_object(target=target, approach_axis=approach_axis) - gripper_controller.disconnect_gripper() - self.home(home) - except Exception as er: - self.logger.error(er) - finally: - gripper_controller.disconnect_gripper() - - def robotiq_screwdriver_transfer( - self, - home: Union[LocationArgument, list] = None, - source: Union[LocationArgument, list] = None, - target: Union[LocationArgument, list] = None, - source_approach_axis: str = None, - target_approach_axis: str = None, - source_approach_distance: float = None, - target_approach_distance: float = None, - ) -> None: - """ - Make a screw transfer using the Robotiq Screwdriver. This function uses linear motions to perform the pick and place movements. - - Args - home (Union[LocationArgument, list]): Home location - source (Union[LocationArgument, list]): Source location - target(Union[LocationArgument, list]): Target location - source_approach_axis (str): Source approach axis (X/Y/Z) - target_approach_axis (str): Target approach axis (X/Y/Z) - source_approach_distance (float): Source approach distance. Unit meters. - target_approach_distance(float): Target approach distance. Unit meters. - """ - self.home(home) - - try: - sr = ScrewdriverController( - hostname=self.hostname, - ur=self.ur_connection, - ur_dashboard=self.ur_dashboard, - ) - sr.screwdriver.activate_screwdriver() - sr.transfer( - source=source, - target=target, - source_approach_axis=source_approach_axis, - target_approach_axis=target_approach_axis, - source_approach_dist=source_approach_distance, - target_approach_dist=target_approach_distance, - ) - sr.screwdriver.disconnect() - except Exception as err: - self.logger.error(err) - - self.home(home) - - def pipette_transfer( - self, - home: Union[LocationArgument, list] = None, - tip_loc: Union[LocationArgument, list] = None, - tip_trash: Union[LocationArgument, list] = None, - source: Union[LocationArgument, list] = None, - target: Union[LocationArgument, list] = None, - volume: int = 10, - pipette_speed: int = 150, - ) -> None: - """ - Make a liquid transfer using the pipette. This function uses linear motions to perform the pick and place movements. - - Args - home (Union[LocationArgument, list]): Home location joint values - tip_loc (Union[LocationArgument, list]): Pipette tip location - tip_trash (Union[LocationArgument, list]): Tip trash location - source (str): Source location - target (str): Target location - volume (int): Pipette transfer volume. Unit number of steps. Each step is 1 mL - """ - if not source or not target: - raise Exception("Please provide both the source and target loactions to make a transfer") - - try: - pipette = TricontinentPipetteController( - hostname=self.hostname, - ur=self.ur_connection, - pipette_ip=self.hostname, - resource_client=self.resource_client, - pipette_resource_id=self.tool_resource_id, - ) - pipette.connect_pipette() - if tip_loc: - pipette.pick_tip(tip_loc=tip_loc) - self.home(home) - pipette.transfer_sample( - home=home, - sample_aspirate=source, - sample_dispense=target, - volume=volume, - speed=pipette_speed, - ) - if tip_trash: - pipette.eject_tip(eject_tip_loc=tip_trash, approach_axis="y") - pipette.disconnect_pipette() - self.logger.info("Disconnecting from the pipette") - except Exception as err: - self.logger.error(err) - - def pipette_pick_and_move_sample( - self, - home: Union[LocationArgument, list] = None, - linear_motion: bool = False, - safe_waypoint: Union[LocationArgument, list] = None, - tip_loc: Union[LocationArgument, list] = None, - sample_loc: Union[LocationArgument, list] = None, - target: Union[LocationArgument, list] = None, - volume: int = 10, - pipette_speed: int = 150, - ) -> None: - """Pipette pick sample from the source location and transfer it to the target location - - Args - home (Union[LocationArgument, list]): Home location use Linear motions if needed - safe_waypoint (Union[LocationArgument, list]): Safe waypoint location to move the pipette - tip_loc (Union[LocationArgument, list]): Pipette tip location - sample_loc (Union[LocationArgument, list]): Sample location - target (Union[LocationArgument, list]): Target location - volume (int): Pipette transfer volume. Unit number of steps. Each step is 1 mL - """ - if not sample_loc or not target: - raise Exception("Please provide both the sample and target loactions to make a transfer") - - try: - pipette = TricontinentPipetteController( - hostname=self.hostname, - ur=self.ur_connection, - pipette_ip=self.hostname, - resource_client=self.resource_client, - pipette_resource_id=self.tool_resource_id, - ) - pipette.connect_pipette(speed=pipette_speed) - pipette.initialize_pipette() - if tip_loc: - pipette.pick_tip(tip_loc=tip_loc) - if home: - self.home(home, linear_motion=linear_motion) - pipette.pick_and_move( - safe_waypoint=safe_waypoint, - sample_loc=sample_loc, - target=target, - volume=volume, - ) - pipette.disconnect_pipette() - self.logger.info("Disconnecting from the pipette") - except Exception as err: - self.logger.error(err) - - def pipette_dispense_and_retrieve( - self, - home: Union[LocationArgument, list] = None, - linear_motion: bool = False, - safe_waypoint: Union[LocationArgument, list] = None, - tip_trash: Union[LocationArgument, list] = None, - target: Union[LocationArgument, list] = None, - volume: int = 10, - pipette_speed: int = 150, - ) -> None: - """Dispense a sample using the pipette. This function uses linear motions to perform the pick and place movements. - Args - home (Union[LocationArgument, list]): Home location joint values - tip_trash (Union[LocationArgument, list]): Tip trash location - target (Union[LocationArgument, list]): Target location - volume (int): Pipette transfer volume. Unit number of steps. Each step is 1 mL - """ - if not target: - raise Exception("Please provide the target loaction to make a dispense") - - try: - pipette = TricontinentPipetteController( - hostname=self.hostname, - ur=self.ur_connection, - pipette_ip=self.hostname, - resource_client=self.resource_client, - pipette_resource_id=self.tool_resource_id, - ) - pipette.connect_pipette(speed=pipette_speed) - pipette.dispense_and_retrieve( - target=target, - safe_waypoint=safe_waypoint, - volume=volume, - ) - if tip_trash: - pipette.eject_tip(eject_tip_loc=tip_trash, approach_axis="y") - pipette.disconnect_pipette() - self.home(home, linear_motion=linear_motion) - self.logger.info("Disconnecting from the pipette") - except Exception as err: - self.logger.error(err) - - def run_droplet( - self, - home: Union[LocationArgument, list] = None, - tip_loc: Union[LocationArgument, list] = None, - sample_loc: Union[LocationArgument, list] = None, - droplet_loc: Union[LocationArgument, list] = None, - tip_trash: Union[LocationArgument, list] = None, - ) -> None: - """Run the full droplet protocol cycle - - Args - home (Union[LocationArgument, list]): Home location - tip_loc (Union[LocationArgument, list]): Pipette tip location - sample_loc (Union[LocationArgument, list]): Sample location - droplet_loc (Union[LocationArgument, list]): Location where the droplet will be hung - tip_trash (Union[LocationArgument, list]): Pipette tip trash location - """ - pipette = OTPipetteController(ur_connection=self.ur_connection, IP=self.hostname) - pipette.connect_pipette() - - self.home(home) - pipette.pick_tip(tip_loc=tip_loc) - pipette.transfer_sample(sample_loc=sample_loc) - self.home(home) - pipette.create_droplet(droplet_loc=droplet_loc) - self.home(home) - pipette.empty_tip(sample_loc=sample_loc) - pipette.eject_tip(eject_tip_loc=tip_trash) - self.home(home) - pipette.disconnect_pipette() - - def run_urp_program(self, transfer_file_path: str = None, program_name: str = None): - """Transfers the urp programs onto the polyscope and initiates them - - Args: - trasnfer_file_path (str): Local file path - program_name (str): Name of the file - """ - if not program_name: - raise ValueError("Provide program name!") - - ur_program_path = "/programs/" + program_name - - if transfer_file_path: - self.ur_dashboard.transfer_program(local_path=transfer_file_path, remote_path=ur_program_path) - sleep(2) - - self.ur_dashboard.load_program(program_path=ur_program_path) - sleep(2) - self.ur_dashboard.run_program() - sleep(5) - - self.logger.info(f"Running the URP program: {program_name}") - time_elapsed = 0 - - program_status = "BUSY" - ready_status_count = 0 - while program_status == "BUSY": - if self.get_movement_state() == "READY": - ready_status_count += 1 - if ready_status_count >= 6: - program_status = "READY" - else: - ready_status_count = 0 - sleep(3) - - program_log = { - "output_code": "0", - "output_msg": "Successfully finished " + program_name, - "output_log": "seconds_elapsed:" + str(time_elapsed), - } - - return program_log diff --git a/src/ur_interface/ur_controller.py b/src/ur_interface/ur_controller.py new file mode 100644 index 0000000..d735014 --- /dev/null +++ b/src/ur_interface/ur_controller.py @@ -0,0 +1,211 @@ +#!/usr/bin/env python3 +"""Interface for UR Driver""" + +import logging +import socket +import traceback +from math import radians +from time import sleep +from typing import Optional, Union + +import math3d as m3 +import numpy as np +from madsci.client.resource_client import ResourceClient +from madsci.common.types.auth_types import OwnershipInfo +from madsci.common.types.location_types import LocationArgument +from urx import Robot + +from ur_interface.ur_error_types import URConnectionError + + +class URController: + """ + This is the primary class for UR robots. + It integrates various interfaces to achieve comprehensive control, encompassing robot initialization via the UR dashboard, + robot motion using URx, and the management of robot end-effectors such as grippers, screwdrivers, electronic pipettes, and cameras." + """ + + def __init__( + self, + hostname: str = None, + resource_client: ResourceClient = None, + resource_owner: OwnershipInfo = None, + tool_resource_id: str = None, + tcp_pose: list = [0, 0, 0, 0, 0, 0], + base_reference_frame: list = None, + logger: Optional[logging.Logger] = None, + ): + """Constructor for the UR class. + :param hostname: Hostname or ip. + :param logger: Logger object for logging messages + """ + + if not hostname: + raise TypeError("Hostname cannot be None Type!") + + self.hostname = hostname + self.resource_client = resource_client + self.tool_resource_id = tool_resource_id + self.resource_owner = resource_owner + self.logger = logger or self._setup_logger() + + self.acceleration = 0.5 + self.velocity = 0.5 + self.robot_current_joint_angles = None + self.gripper_speed: int = None + self.gripper_force: int = None + + try: + self.ur_connection = self.connect_ur() + self.ur_connection.set_tcp(tcp_pose) + if base_reference_frame: + self._set_base_reference_frame(base_reference_frame) + self.get_movement_state() + + except Exception as e: + self.logger.error(f"Failed to initialize UR: {e}\n{traceback.format_exc()}") + raise e + + def connect_ur(self) -> Robot: + """Create connection to the UR robot""" + for attempt in range(10): + try: + self.logger.info(f"Attempting robot connection (attempt {attempt + 1}/10)...") + robot_connection = Robot(self.hostname) + + except socket.error as e: + self.logger.warning(f"Robot connection attempt {attempt + 1} failed: {e}") + sleep(1) + + except Exception as e: + self.logger.error(f"Unexpected error during robot connection: {e}\n{traceback.format_exc()}") + sleep(1) + + else: + self.logger.info("Successful UR connection") + return robot_connection + + raise URConnectionError(f"Failed to connect to UR robot at {self.hostname} after 10 attempts") + + def disconnect_ur(self): + """ + Description: Disconnects the socket connection with the UR robot + """ + try: + if self.ur_connection: + self.ur_connection.close() + self.logger.info("Robot connection closed successfully") + except Exception as e: + self.logger.error(f"Error closing robot connection: {e}\n{traceback.format_exc()}") + raise e + + def _setup_logger(self) -> logging.Logger: + """Setup default logger if none provided""" + logger = logging.getLogger(__name__) + logger.setLevel(logging.INFO) + if not logger.handlers: + handler = logging.StreamHandler() + formatter = logging.Formatter("%(asctime)s - %(name)s - %(levelname)s - %(message)s") + handler.setFormatter(formatter) + logger.addHandler(handler) + return logger + + def disconnect(self): + """Disconnects the robot from URX and UR Dashboard connections""" + self.ur.disconnect_ur() + self.ur_dashboard.clear_operational_mode() + self.ur_dashboard.disconnect() + self.logger.info("UR disconnected successfully") + + def _set_base_reference_frame(self, base_reference_frame: list) -> None: + """Sets the base reference frame for the robot. + Args: + base_reference_frame (list): 6 element reference frame. [x, y, z, rx, ry, rz]. Rotation values are in degrees, later converted to radians in the function. + """ + if not isinstance(base_reference_frame, list) or len(base_reference_frame) != 6: + raise ValueError("Base reference frame must be a list of 6 values") + + # Extract position and rotation components + x, y, z, rx_deg, ry_deg, rz_deg = base_reference_frame + + # Create translation vector (only if any translation values are non-zero) + if any([x, y, z]): + translation = m3.Vector(x, y, z) + else: + translation = m3.Vector(0, 0, 0) + + # Start with identity rotation + rotation = m3.Orientation() # Identity rotation + + # Apply only non-zero rotations in order + if rx_deg != 0: + rx_rad = radians(rx_deg) + rotation = rotation * m3.Orientation.new_rot_x(rx_rad) + + if ry_deg != 0: + ry_rad = radians(ry_deg) + rotation = rotation * m3.Orientation.new_rot_y(ry_rad) + + if rz_deg != 0: + rz_rad = radians(rz_deg) + rotation = rotation * m3.Orientation.new_rot_z(rz_rad) + # Create the transform + transform = m3.Transform(rotation, translation) + + # Set the coordinate system + self.ur_connection.set_csys(transform) + self.logger.info(f"Base reference frame set to: {base_reference_frame}") + + def get_movement_state(self): + """Gets robot movement status by checking robot joint values. + Return (str) READY if robot is not moving + BUSY if robot is moving + """ + current_location = {"joint_angles": self.ur_connection.getj(), "linear_coordinates": self.ur_connection.getl()} + if self.robot_current_joint_angles is None: + movement_state = "READY" + else: + if ( + np.linalg.norm(np.array(current_location["joint_angles"]) - np.array(self.robot_current_joint_angles)) + < 1e-3 + ): + movement_state = "READY" + else: + movement_state = "BUSY" + + self.robot_current_joint_angles = current_location["joint_angles"] + + return movement_state, current_location + + def move_to_location(self, location: Union[LocationArgument, list], linear_motion: bool = False) -> None: + """Moves the robot to the home location. + + Args: home_location: 6 joint value location + """ + try: + self.logger.info("Homing the robot...") + + if linear_motion: + if isinstance(location, LocationArgument): + location = location.representation["linear_coordinates"] + self.ur_connection.movel(location, self.velocity, self.acceleration) + else: + if isinstance(location, LocationArgument): + location = location.representation["joint_angles"] + self.ur_connection.movej(location, self.velocity, self.acceleration) + self.logger.info("Robot moved") + except Exception as e: + self.logger.error(f"Error in moving the robot: {e}\n{traceback.format_exc()}") + raise e + + def set_digital_io(self, channel: int = None, value: bool = None) -> None: + """Sets digital I/O outputs to open an close the channel. This helps controlling the external tools + + Args + channel (int): Channel number + value (bool): False for close, True for open + """ + if channel is None or value is None: + self.logger.error("Channel or value is not specified") + return + self.ur_connection.set_digital_out(channel, value) diff --git a/src/ur_interface/ur_dashboard.py b/src/ur_interface/ur_dashboard.py index b91ca2a..bcd4946 100644 --- a/src/ur_interface/ur_dashboard.py +++ b/src/ur_interface/ur_dashboard.py @@ -7,7 +7,7 @@ from scp import SCPClient, SCPException -class UR_DASHBOARD: +class URDashboard: """ This is a python interface to communicate with the UR Dashboard server. The UR can be controlled remotely by sending simple commands to the GUI over a TCP/IP socket. @@ -374,34 +374,3 @@ def stop_program(self) -> str: Return (str): Socket response """ return self.send_command("stop") - - -if __name__ == "__main__": - """Tests""" - robot = UR_DASHBOARD("146.137.240.38") - - robot.get_loaded_program() - robot.run_program() - # robot.transfer_program("/home/rpl/workspace/ur_module/src/ur_interface/scripts/urp_programs/interpreter_mode.urp","/programs/interpreter_mode.urp") - # robot.get_program_state() - # robot.load_program("/programs/interpreter_mode.urp") - # if "File not found" in a: - # print(a) - # robot.get_overall_robot_status() - # robot.get_operational_mode() - # robot.robot_mode() - # robot.close_popup() - # robot.initialize() - # robot.get_program_run_status() - # robot.load_program("/home/rpl/test.txt") - # robot.get_loaded_program() - # robot.power_on() - # robot.brake_release() - # robot.power_off() - # robot.brake_release() - # robot.self.get_safety_status() - # robot.quit() - # robot.clear_operational_mode() - # robot.transfer_program("/home/rpl/test.urp", "/programs/katerina.urp") - # robot.load_program("/programs/katerina.urp") - # robot.run_program() diff --git a/src/ur_interface/ur_tools/ati_tool_changer_controller.py b/src/ur_interface/ur_tools/ati_tool_changer_controller.py deleted file mode 100644 index e030ef0..0000000 --- a/src/ur_interface/ur_tools/ati_tool_changer_controller.py +++ /dev/null @@ -1,82 +0,0 @@ -"""Remote interface for ATI Tool Changers""" - -import epics - - -class ATIToolChangerController: - """Remotely controls the ATI tool chagers over EPICS PVs""" - - def __init__(self, tool_changer_pv: str = None): - """Constructor""" - self.current_tool = None - self.pv = tool_changer_pv - self.connect_tool_changer() - - def connect_tool_changer(self): - """ - Connect tool changer - """ - - try: - # Establishing a connection with the tool changer on EPICS - self.tool_changer = epics.PV(self.pv) - - except Exception as err: - print("Tool changer error: ", err) - - else: - print("Tool changer is connected.") - - def disconnect_tool_changer(self): - """ - Disconnect tool changer - """ - - try: - # Closing the connection with the tool changer on EPICS - self.tool_changer.disconnect() - - except Exception as err: - print("Tool changer error: ", err) - - else: - print("Tool changer is disconnected.") - - def get_tool_changer_status(self): - """ - Description: - - Gets the tool changer current status. - - Tool changer is controlled by pyepics PV commands. - """ - status = self.tool_changer.get() - return status - - def lock_tool_changer(self): - """ - Description: - - Locks the tool changer. - - Tool changer is controlled by pyepics PV commands. - """ - try: - print("Locking the tool changer...") - self.tool_changer.put(1) - except Exception as err: - print( - "Error accured while locking the tool changer: ", - err, - ) - - def unlock_tool_changer(self): - """ - Description: - - Unlocks the tool changer. - - Tool changer is controlled by pyepics PV commands. - """ - try: - print("Unlocking the tool changer...") - self.tool_changer.put(0) - except Exception as err: - print( - "Error accured while unlocking the tool changer: ", - err, - ) diff --git a/src/ur_interface/ur_tools/best.pt b/src/ur_interface/ur_tools/best.pt deleted file mode 100644 index 93d6a2a..0000000 Binary files a/src/ur_interface/ur_tools/best.pt and /dev/null differ diff --git a/src/ur_interface/ur_tools/camera_controller.py b/src/ur_interface/ur_tools/camera_controller.py deleted file mode 100644 index ec4caa4..0000000 --- a/src/ur_interface/ur_tools/camera_controller.py +++ /dev/null @@ -1,665 +0,0 @@ -"""Remote interface for Realsense camera control""" - -from math import cos, degrees, radians -from time import sleep, time -from typing import List, Optional, Tuple - -import cv2 -import numpy as np -import pyrealsense2 as realsense -from robotiq_gripper_driver import RobotiqGripper -from ultralytics import YOLO -from urx import Robot - - -class CameraController: - """ - CameraController is a class created for UR robots to utilize Intel Realsense D400 series cameras. - It can detect objects based on a pre-trained YOLO model with 5 different labwares, and plan - a robot trajectory to pick up the objects based on distance and object reference frame information obtained from the camera. - """ - - def __init__( - self, - hostname: str = None, - socket_timeout: float = 2.0, - ur_connection: Optional[Robot] = None, - target_object: Optional[str] = None, - ) -> None: - """ - Constructor for the CameraController class. - - Args: - hostname (str): The IP address of the robot. - ur_connection (Robot): The connection to the robot (urx.Robot instance), defaults to None. - target_object (str): The target object for YOLO model, defaults to None. - - Raises: - ValueError: Raised when ur_connection or target_object is not provided. - ValueError: Raised when target_object category doesn't exist in the trained model class list. - """ - if ur_connection is None: - raise ValueError("UR connection is not provided!") - if target_object is None: - raise ValueError("Target object for YOLO model is not provided") - - self.ur_connection = ur_connection - self.target_object = target_object.lower() - self.model = None - self.object_distance = 0 - self.object_reference_frame = None - self.gripper = RobotiqGripper() - - self.MOVE_ACC = 1.0 - self.MOVE_VEL = 1.0 - self.conveyor_speed = 0.0 # Conveyor speed in meters per second - self.CLASS_NAMES = [ - "deepwellplates", - "tipboxes", - "hammers", - "wellplates", - "wellplate_lids", - ] - - self._validate_target_object() - self._connect_to_gripper(hostname) - - def _validate_target_object(self): - if self.target_object not in self.CLASS_NAMES: - raise ValueError( - f"Target object category '{self.target_object}' doesn't exist in the trained model class list" - ) - - def _connect_to_gripper(self, robot_ip: str): - print("Connecting to gripper...") - self.gripper.connect(robot_ip, 63352) - self.gripper.activate() - self.gripper.move_and_wait_for_pos(0, 150, 0) - - def start_camera_stream(self) -> None: - """Start the Intel realsense camera pipeline""" - - try: - self.pipeline = realsense.pipeline() - config = realsense.config() - config.enable_stream( - realsense.stream.color, - 640, - 480, - realsense.format.rgb8, - 30, - ) - config.enable_stream( - realsense.stream.depth, - 640, - 480, - realsense.format.z16, - 30, - ) - self.pipeline.start(config) - except realsense.error as e: - print(f"RealSense error {e.get_failed_function()}: {e.get_failed_args()}") - print(f"{e.get_description()}") - - def capture_image( - self, - ) -> Tuple[np.array, "realsense.frame", "realsense.frame"]: - """ - Capture a new image from the camera. - - Returns: - Tuple[np.array, 'realsense.frame', 'realsense.frame']: The captured image and the color and depth frames. - """ - frames = self.pipeline.wait_for_frames() - color_frame = frames.get_color_frame() - depth_frame = frames.get_depth_frame() - img = np.asanyarray(color_frame.get_data()) - img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR) - img = cv2.resize(img, (640, 480)) - - return img, color_frame, depth_frame - - def load_yolo_model(self, model_path: Optional[str] = None): - """ - Loads the trained YOLO model. - - Args: - model_path (Optional[str]): Path to the model file, defaults to None. - """ - model_file_path = model_path if model_path else "best.pt" - # Load the trained YOLO model - self.model = YOLO(model_file_path) - - def _get_object_predictions(self, img: np.array) -> Tuple[list, list]: - """ - Detects objects in the given image using the provided YOLO model. - - Args: - model (YOLO): The YOLO model to use for object detection. - img (np.array): The image to detect objects in. - - Returns: - Tuple[list, list]: A tuple containing a list of detected boxes and their corresponding classes. - """ - prediction = self.model(img)[0] - boxes = prediction.boxes - classes = prediction.cls - - return boxes, classes - - # This function needs to be implemented: - def object_is_within_threshold(self, object_point: Tuple[float, float, float]) -> bool: - """ - Determines whether the object is within a certain distance threshold for picking up. - - Args: - object_point (Tuple[float, float, float]): The 3D coordinates of the object point. - - Returns: - bool: True if the object is within the threshold, otherwise False. - """ - threshold = 0.1 # This is an example value and should be adjusted based on your specific requirements - distance = np.linalg.norm(np.array(object_point)) - return distance <= threshold - - def _calculate_object_xy(self, boxes) -> Optional[Tuple[int, int]]: - """ - Calculates the center of the first detected object if any objects are detected. - - Args: - boxes (List[Box]): A list of detected objects represented as boxes. - - Returns: - Optional[Tuple[int, int]]: The x and y coordinates of the first object's center if detected, otherwise None. - """ - if len(boxes) > 0: - xmin, ymin, xmax, ymax = boxes[0].xyxy[0] - center_x = int((xmin + xmax) / 2) - center_y = int((ymin + ymax) / 2) - return center_x, center_y - else: - return None - - def get_object_xy(self, timeout=10) -> Optional[Tuple[int, int]]: - """ - Aligns the robot arm to the object's center until the center is detected. - - Args: - timeout (int): The maximum amount of time (in seconds) to try aligning the robot arm. - - Returns: - Optional[Tuple[int, int]]: The x and y pixel coordinates of the object's center if detected, otherwise None. - """ - object_xy = None - start_time = time() - while object_xy is None and time() - start_time < timeout: - img, color_frame, depth_frame = self.capture_image() - - if not color_frame or not depth_frame or not img: - return None - - boxes, classes = self._get_object_predictions(img) - object_xy = self._calculate_object_xy(boxes) - - return object_xy - - def _calculate_object_reference_frame( - self, - depth_frame: "realsense.frame", - center_x: int, - center_y: int, - ): - """ - Get the object reference frame from the depth frame and center of the bounding box. - - Args: - depth_frame (frame): Depth frame of the image. - center_x (int): X coordinate of the object center. - center_y (int): Y coordinate of the object center. - """ - - # Get the intrinsic parameters of the depth frame - depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics - # Use the depth value and the center of the bounding box to get the 3D coordinates of the object - self.object_reference_frame = realsense.rs2_deproject_pixel_to_point( - depth_intrin, - [center_x, center_y], - self.object_distance, - ) - - @staticmethod - def _calculate_box_center(xmin: float, xmax: float, ymin: float, ymax: float) -> Tuple[int, int]: - return int((xmin + xmax) / 2), int((ymin + ymax) / 2) - - @staticmethod - def _draw_on_image( - img: np.array, - xmin: float, - ymin: float, - xmax: float, - ymax: float, - center_x: int, - center_y: int, - distance: float, - ) -> None: - cv2.rectangle( - img, - (int(xmin), int(ymin)), - (int(xmax), int(ymax)), - (0, 255, 0), - 2, - ) - cv2.putText( - img, - f"{distance:.2f}m", - (int(xmin), int(ymin) - 10), - cv2.FONT_HERSHEY_SIMPLEX, - 0.9, - (0, 255, 0), - 2, - ) - cv2.circle(img, (center_x, center_y), 5, (0, 0, 255), -1) - cv2.circle(img, (640 / 2, 480 / 2), 5, (0, 0, 255), -1) - - def move_to_object(self, move_z: Optional[float] = 0.0): - """Method to move the robot arm to the object""" - - # Extract the x, y, and z coordinates - trans_x = self.object_reference_frame[0] - trans_y = self.object_reference_frame[1] - trans_z = self.object_reference_frame[2] - - # Print out the object's 3D coordinates - print("Object XYZ: " + str(self.object_reference_frame)) - - if trans_z != 0: - # Move the robot's tool (e.g. a gripper) to the object - self.ur_connection.translate_tool( - [-trans_x, -trans_y, move_z], - acc=self.MOVE_ACC, - vel=self.MOVE_VEL, - ) - - def _detect_object_coordinates(self, img: np.array, depth_frame: "realsense.frame"): - """ - This function takes an image and a depth frame as input, runs the object detection model on the image, - and checks the classes of detected objects. If the class of a detected object matches the target object, - it calculates the center of this object, gets its distance, and draws on the image. Then, it calculates - the object reference frame based on the center of the object and depth frame and commands the robot to - move to this object. If the target object is not found in the image, it logs a message. - - Args: - img (np.array): The image from the camera. - depth_frame ('realsense.frame'): The depth frame from the camera. - """ - - boxes, classes = self._get_object_predictions(img) - for (xmin, ymin, xmax, ymax), cls in zip(boxes.xyxy, classes): - if cls == self.target_object: - center_x, center_y = self._calculate_box_center(xmin, xmax, ymin, ymax) - self.object_distance = depth_frame.get_distance(center_x, center_y) - - self._draw_on_image( - img, - xmin, - ymin, - xmax, - ymax, - center_x, - center_y, - self.object_distance, - ) - - self._calculate_object_reference_frame(depth_frame, center_x, center_y) - break - else: - raise Exception(f"Target object {self.target_object} not found in the frame.") - - def calculate_object_alignment(self) -> None: - """ - Method to center the robot gripper over the detected object in its field of view. - - Args: - object_xy (tuple): Optional; pre-calculated object center coordinates. - - Returns: - None - """ - - # Capture image and get color and depth frames - img, color_frame, depth_frame = self.capture_image() - if not color_frame or not depth_frame or not img: - raise ValueError("Could not capture image or retrieve color/depth frames") - - self._detect_object_coordinates(img, depth_frame) - - def move_over_object(self, object_point: Tuple[float, float, float]) -> None: - """ - Moves the robot arm over the detected object. - - Args: - object_point (Tuple[float, float, float]): The 3D coordinates of the object point. - """ - adjacent_length = self._get_adjacent_length(object_point) - - self._move_gripper_perpendicular() - - desired_position = adjacent_length - self.ur_connection.translate_tool([0, desired_position, 0], 1, 0.2) - - def _get_adjacent_length(self, object_point: Tuple[float, float, float]) -> float: - """ - Calculates the adjacent length to the object from the robot arm. - - Args: - object_point (Tuple[float, float, float]): The 3D coordinates of the object point. - - Returns: - float: The calculated adjacent length. - """ - trans_z = object_point[2] - angle = self.ur_connection.getl()[4] - - adjacent_length = cos(degrees(angle)) * trans_z - print(f"Adjacent length: {adjacent_length}") - - return abs(adjacent_length) - - def _move_gripper_perpendicular(self) -> None: - """ - Adjusts the gripper to a perpendicular orientation. - """ - - current_orientation = self.ur_connection.get_orientation() - euler_angles = current_orientation.to_euler(encoding="xyz") - print(euler_angles) - move_rx = 3.14 - abs(euler_angles[0]) - - if euler_angles[1] < 0: - move_ry = abs(euler_angles[1]) - else: - move_ry = -(euler_angles[1]) - - current_orientation.rotate_xt(move_rx) - current_orientation.rotate_yt(-move_ry) - # print(move_ry) - - self.ur_connection.set_orientation(current_orientation, 0.2, 0.2) - - def find_frame_areas(self, boxes) -> List[float]: - """ - Determines the areas of bounding boxes of the detected objects. - - Args: - boxes (List[Box]): A list of detected objects represented as boxes. - - Returns: - List[float]: A list of areas of each bounding box. - """ - areas = [] - for box in boxes: - xmin, ymin, xmax, ymax = box.xyxy[0] - areas.append((xmax - xmin) * (ymax - ymin)) # Area = width * height - return areas - - def align_gripper(self) -> None: - """ - Rotates the image and aligns the gripper with the target object until a minimum bounding box area is found. - - Returns: - None - """ - img, _, _ = self.capture_image() - image_rotation_angle = 1 - robot_rotation_angle = 0 - smallest_frame_area = float("inf") - - rotate = True # introduce a flag to control the while loop - - while rotate: - rotation_matrix = cv2.getRotationMatrix2D( - (img.shape[1] // 2, img.shape[0] // 2), - image_rotation_angle, - 1.0, - ) - rotated_img = cv2.warpAffine( - img, - rotation_matrix, - (img.shape[1], img.shape[0]), - ) - - boxes, classes = self._get_object_predictions(rotated_img) - for i, cls in enumerate(classes): - if cls != self.target_object: - continue - frame_areas = self.find_frame_areas([boxes[i]]) - - current_frame_area = min(frame_areas) - if current_frame_area < smallest_frame_area: - smallest_frame_area = current_frame_area - robot_rotation_angle = image_rotation_angle - elif image_rotation_angle > 45 and smallest_frame_area < current_frame_area: - rotate = False # if the smallest frame area is found, stop the while loop - break - image_rotation_angle += 1 - - self.ur_connection.movej( - self.ur_connection.getj()[:-1] + [radians(robot_rotation_angle)], - acc=0.2, - vel=0.2, - ) - - def pick_static_object(self) -> None: - """ - Detects, aligns to, and picks up an object using the robot arm and gripper. - """ - - for i in range(6): # noqa - object_xy = self.get_object_xy() - - if object_xy: - self.calculate_object_alignment() - print(f"OBJECT_POINT: {self.object_reference_frame}") - self.move_to_object() - else: - self.object_reference_frame = None - - if not self.object_reference_frame: - print("Object can't be found!") - return - - sleep(4) - - self.ur_connection.translate_tool([0.02, 0.09, 0], 1, 0.2) - self.ur_connection.translate_tool( - [0, 0, self.object_reference_frame[2] - 0.16], - 1, - 0.2, - ) - self.gripper.move_and_wait_for_pos(160, 150, 100) - self.ur_connection.translate_tool( - [0, 0, -(self.object_reference_frame[2] - 0.2)], - 1, - 0.2, - ) - - waypoint = [ - 0.2655990719795227, - -1.7126232586302699, - -1.7399795055389404, - -1.254279003744461, - -4.749170009289877, - -2.394965473805563, - ] - drop_off_above = [ - -1.4304960409747522, - -1.0302266043475647, - -2.2368316650390625, - -1.4599171516350289, - -4.7227471510516565, - -3.00033146539797, - ] - drop_off = [ - -1.4450705687152308, - -1.3130722504905243, - -2.613124132156372, - -0.8007843655398865, - -4.7251179854022425, - -3.009803597127096, - ] - - self.ur_connection.movej(waypoint, 0.5, 0.5) - self.ur_connection.movej(drop_off_above, 0.5, 0.5) - self.ur_connection.movej(drop_off, 0.5, 0.5) - - self.gripper.move_and_wait_for_pos(0, 150, 100) - - self.ur_connection.movej(drop_off_above, 0.5, 0.5) - self.ur_connection.movej(waypoint, 0.5, 0.5) - - def pick_dynamic_object(self): - """Picks up dynamicly moving objects. - Keeps aligning and attemps to pick up at the same time till object is actually picked up. - When the align object is called, robot arm also moved towards thte object slowly (0.1 each time) within each loop - Once robot gets to certain distance it would only perform pick movement quickly. - - """ - object_grasped = False - while not object_grasped: - object_xy = self.get_object_xy() - - if object_xy: - self.calculate_object_alignment() - print(f"OBJECT_POINT: {self.object_reference_frame}") - if self.object_reference_frame[2] > 0.31: - self.move_to_object(move_z=0.01) - elif self.object_reference_frame[2] < 0.31: - self.ur_connection.translate_tool( - [0.02, 0.09, 0.25], - self.MOVE_ACC, - self.MOVE_VEL, - ) - self.gripper.move_and_wait_for_pos(160, 150, 100) - self.ur_connection.translate_tool( - [0, 0, -0.25], - self.MOVE_ACC, - self.MOVE_VEL, - ) - object_grasped = True - else: - # TODO: Try keeping the same pixel location for depth sensing to contunie the pick up movement - # if the object was initially detected but throughout the movement process detection cannot be made because of the camera errors. - # - self.object_reference_frame = None - - if not self.object_reference_frame: - print("Object can't be found!") - return - - def pick_conveyor_object(self): - """pick conveyor object""" - MAX_ATTEMPTS = 5 - num_failed_attempts = 0 - while True: - object_xy = self.get_object_xy() - - if object_xy: - self.calculate_object_alignment(object_xy) - - if self.conveyor_speed != 0: - # If the conveyor belt is moving, adjust the target point based on its speed - pickup_delay = self._estimate_pickup_delay( - self.object_reference_frame - ) # Calculate the delay before the robot arm can pick up the object - self.object_reference_frame[1] += ( - self.conveyor_speed * pickup_delay - ) # Adjust the target y-coordinate based on the conveyor speed and pickup delay - - self.move_over_object(self.object_reference_frame) - - sleep(5) - - self.align_gripper() - self.grip_object() - - if self.has_picked_up_object(): - break - - num_failed_attempts += 1 - if num_failed_attempts > MAX_ATTEMPTS: - print("Failed to pick up object after multiple attempts.") - break - - def _estimate_pickup_delay(self, object_point: Tuple[float, float, float]) -> float: - """ - Estimates the time it will take for the robot arm to reach the target point. - For simplicity, this example assumes a constant time delay. In a real application, you might want to calculate this based on the robot's current state and the target point. - """ - robot_position = self.ur_connection.getl() - distance_to_target = ( - (robot_position[0] - object_point[0]) ** 2 - + (robot_position[1] - object_point[1]) ** 2 - + (robot_position[2] - object_point[2]) ** 2 - ) ** 0.5 - time = distance_to_target / self.MOVE_VEL - return time - - # Uncompleted - def calculate_3d_orientation(self, depth_frame): - """Calculates the 3D orientation of the object""" - # Obtain depth data - depth_data = np.asanyarray(depth_frame.get_data()) - # Assuming that we already have the bounding box of the object - box = 1 # Bounding box of the detected object - # Extract the depth information for the object from the depth data - object_depth_data = depth_data[box[1] : box[3], box[0] : box[2]] # noqa - # Find the 3D orientation based on the object's depth data - orientation = 1 # noqa - # Function to calculate orientation based on depth data - # return orientation - pass - - # Uncompleted - def align_gripper_to_object(self): - """Alligns the gripper with the target object before picking it up""" - # Capture image - _, _, depth_frame = self.capture_image() - # Calculate 3D orientation of the object - orientation = self.calculate_3d_orientation(depth_frame) - # Align the gripper to the object's 3D orientation - self.ur_connection.set_orientation(orientation, acc=0.2, vel=0.2) - pass - - -def main(): - """Main function run camera controlled Pick & Place""" - # Initialize CameraController - robot_ip = "192.168.1.10" # replace with your robot's IP - ur_robot = Robot(robot_ip) # Initialize the UR robot connection - target_object = "wellplates" # replace with the object you want to pick - controller = CameraController(robot_ip, ur_robot, target_object) - - # Load model and start streaming - controller.load_yolo_model("best.pt") # replace with your model's path - controller.start_camera_stream() - - for i in range(6): # noqa - object_xy = controller.get_object_xy() - - if object_xy: - controller.calculate_object_alignment(object_xy) - print( - "OBJECT_POINT: ", - controller.object_reference_frame, - ) - - controller.move_over_object(controller.object_reference_frame) - sleep(5) - - controller.align_gripper() - controller.pick_static_object() - controller.pipeline.stop() - - -if __name__ == "__main__": - main() diff --git a/src/ur_interface/ur_tools/gripper_controller.py b/src/ur_interface/ur_tools/gripper_controller.py deleted file mode 100644 index 4616cf7..0000000 --- a/src/ur_interface/ur_tools/gripper_controller.py +++ /dev/null @@ -1,562 +0,0 @@ -"""Controls Various Type of Gripper End Effectors""" - -import logging -import socket -import traceback -from copy import deepcopy -from math import radians -from time import sleep -from typing import Union - -from madsci.common.types.location_types import LocationArgument - -from ur_interface.ur_error_types import GripperConnectionError, GripperOperationError - -from .robotiq_gripper_driver import RobotiqGripper - - -class FingerGripperController: - """Controls Robotiq Finger Grippers""" - - def __init__( - self, - hostname: str = "146.137.240.38", - port: int = 63352, - ur=None, - resource_client=None, - gripper_resource_id: str = None, - logger: logging.Logger = None, - ): - """ - Constructor for the FingerGripperController class. - - Args: - hostname (str): The hostname of the robot. - port (int): Port number to connect to the robot over the Interpreter socket - """ - self.host = hostname - self.PORT = port - self.resource_client = resource_client - self.gripper_resource_id = gripper_resource_id - self.logger = logger - - if not ur: - raise Exception("UR connection is not established") - else: - self.ur = ur - # self.ur.set_payload(1.2)# TODO: Check the actual payload - - self.gripper_close = 255 # 0-255 (255 is closed) - self.gripper_open = 0 - self.gripper_speed = 255 # 0-255 - self.gripper_force = 255 # 0-255 - - self.acceleration = 0.7 - self.velocity = 0.7 - self.speed_ms = 0.750 - self.speed_rads = 0.750 - self.accel_mss = 1.200 - self.accel_radss = 1.200 - self.blend_radius_m = 0.001 - self.ref_frame = [0, 0, 0, 0, 0, 0] - - def __del__(self): - """Destructor for the FingerGripperController class.""" - try: - self.disconnect_gripper() - except Exception as e: - self.logger.error(f"Error during gripper disconnection in destructor: {e}\n{traceback.format_exc()}") - raise e - - def connect_gripper(self, max_retries: int = 2): - """ - Connect to the gripper - """ - for attempt in range(max_retries): - try: - # GRIPPER SETUP: - self.logger.info(f"Connecting to gripper (attempt {attempt + 1}/{max_retries})...") - self.gripper = RobotiqGripper() - - self.logger.debug(f"Attempting socket connection to {self.host}:{self.PORT}") - self.gripper.connect(hostname=self.host, port=self.PORT, socket_timeout=5) - - if self.gripper.is_active(): - self.logger.info("Gripper already active") - else: - self.logger.info("Activating gripper...") - self.gripper.activate() - self.logger.info("Opening gripper...") - self.open_gripper() - - self.logger.info("Gripper is ready") - return - - except socket.timeout as e: - self.logger.error(f"Socket timeout on attempt {attempt + 1}: {e}") - if attempt < max_retries - 1: - self.logger.info("Attempting to reset tool communication...") - try: - self.ur.set_tool_communication( - baud_rate=115200, - parity=0, - stop_bits=1, - rx_idle_chars=1.5, - tx_idle_chars=3.5, - ) - sleep(4) - except Exception as reset_error: - self.logger.error( - f"Error resetting tool communication: {reset_error}\n{traceback.format_exc()}" - ) - - except socket.error as e: - self.logger.error(f"Socket error on attempt {attempt + 1}: {e}\n{traceback.format_exc()}") - if attempt < max_retries - 1: - self.logger.info("Retrying connection...") - sleep(2) - - except Exception as e: - self.logger.error( - f"Unexpected error connecting to gripper on attempt {attempt + 1}: {e}\n{traceback.format_exc()}" - ) - if attempt < max_retries - 1: - sleep(2) - - raise GripperConnectionError(f"Failed to connect to gripper after {max_retries} attempts") # noqa - - def disconnect_gripper(self): - """ - Discconect from the gripper - """ - try: - if self.gripper: - self.logger.info("Disconnecting gripper...") - self.gripper.disconnect() - self.logger.info("Gripper connection closed") - except Exception as e: - self.logger.error(f"Error disconnecting gripper: {e}\n{traceback.format_exc()}") - raise - - def home_robot(self, home: Union[LocationArgument, list] = None) -> None: - """ - Home the robot - """ - if not home: - return - try: - self.logger.info("Homing robot to specified position...") - if isinstance(home, LocationArgument): - home_location = home.representation - elif isinstance(home, list): - home_location = home - else: - raise Exception("Please provide an accurate home location") - - self.logger.debug(f"Homing robot to: {home_location}") - self.ur.movej(home_location, self.acceleration, self.velocity) - except Exception as e: - self.logger.error(f"Error homing robot: {e}\n{traceback.format_exc()}") - raise - - def open_gripper( - self, - pose: float = None, - speed: float = None, - force: float = None, - ) -> None: - """Opens the gripper using pose, speed and force variables""" - try: - if pose: - self.gripper_open = pose - if force: - self.gripper_force = force - if speed: - self.gripper_speed = speed - - self.logger.info(f"Opening gripper to position: {self.gripper_open}") - - self.gripper.move_and_wait_for_pos( - self.gripper_open, - self.gripper_speed, - self.gripper_force, - ) - sleep(0.5) - self.logger.debug("Gripper opened successfully") - - except socket.timeout as e: - self.logger.error(f"Timeout while opening gripper: {e}") - raise GripperOperationError(f"Gripper open operation timed out: {e}") # noqa - - except Exception as e: - self.logger.error(f"Error opening gripper: {e}\n{traceback.format_exc()}") - raise GripperOperationError(f"Failed to open gripper: {e}") # noqa - - def close_gripper( - self, - pose: float = None, - speed: float = None, - force: float = None, - ) -> None: - """Closes the gripper using pose, speed and force variables""" - try: - if pose: - self.gripper_close = pose - if force: - self.gripper_force = force - if speed: - self.gripper_speed = speed - self.logger.info(f"Closing gripper to position: {self.gripper_close}") - - self.gripper.move_and_wait_for_pos( - self.gripper_close, - self.gripper_speed, - self.gripper_force, - ) - sleep(0.5) - self.logger.debug("Gripper closed successfully") - - except socket.timeout as e: - self.logger.error(f"Timeout while closing gripper: {e}") - raise GripperOperationError(f"Gripper close operation timed out: {e}") # noqa - - except Exception as e: - self.logger.error(f"Error closing gripper: {e}\n{traceback.format_exc()}") - raise GripperOperationError(f"Failed to close gripper: {e}") # noqa - - def pick( - self, - source: Union[LocationArgument, list] = None, - approach_axis: str = None, - approach_distance: float = None, - ): - """Pick up from first goal position""" - try: - if isinstance(source, LocationArgument): - source_location = source.representation - elif isinstance(source, list): - source_location = source - else: - raise Exception("Please provide an accurate source location") - - if not approach_distance: - approach_distance = 0.05 - - axis = None - - if not approach_axis or approach_axis.lower() == "z": - axis = 2 - elif approach_axis.lower() == "y": - axis = 1 - elif approach_axis.lower() == "-y": - axis = 1 - approach_distance = -approach_distance - elif approach_axis.lower() == "x": - axis = 0 - elif approach_axis.lower() == "-x": - axis = 0 - approach_distance = -approach_distance - - above_goal = deepcopy(source_location) - above_goal[axis] += approach_distance - - self.logger.info(f"Starting pick operation from source: {source_location}") - - self.open_gripper() - - self.logger.debug("Moving to above goal position") - self.ur.movel(above_goal, self.acceleration, self.velocity) - - self.logger.debug("Moving to goal position") - self.ur.movel(source_location, self.acceleration, self.velocity) - - self.close_gripper() - - if self.resource_client and isinstance(source, LocationArgument): # Handle resources if configured - try: - popped_object, updated_resource = self.resource_client.pop(resource=source.resource_id) - self.resource_client.push(resource=self.gripper_resource_id, child=popped_object) - except Exception as e: - self.logger.error(f"Resource client error during pick: {e}\n{traceback.format_exc()}") - - self.logger.debug("Moving back to above goal position") - self.ur.movel(above_goal, self.acceleration, self.velocity) - self.logger.info("Pick operation completed successfully") - - except GripperOperationError: - raise - except Exception as e: - self.logger.error(f"Error during pick operation: {e}\n{traceback.format_exc()}") - raise GripperOperationError(f"Pick operation failed: {e}") # noqa - - def pick_screw( - self, - screw_loc: Union[LocationArgument, list] = None, - ) -> None: - """Handles the pick screw request""" - - if isinstance(screw_loc, LocationArgument): - source_location = screw_loc.representation - elif isinstance(screw_loc, list): - source_location = screw_loc - - above_goal = deepcopy(source_location) - above_goal[2] += 0.06 - self.ur.movel(above_goal, self.acceleration, self.velocity) - self.ur.movel(source_location, 0.2, 0.2) - self.ur.movel(above_goal, self.acceleration, self.velocity) - - def place( - self, - target: Union[LocationArgument, list] = None, - approach_axis: str = None, - approach_distance: float = None, - ): - """Place down at second goal position""" - try: - if isinstance(target, LocationArgument): - target_location = target.representation - elif isinstance(target, list): - target_location = target - else: - raise ValueError("Please provide an accurate target location") - - if not approach_distance: - approach_distance = 0.05 - - axis = None - - if not approach_axis or approach_axis.lower() == "z": - axis = 2 - elif approach_axis.lower() == "y": - axis = 1 - elif approach_axis.lower() == "-y": - axis = 1 - approach_distance = -approach_distance - elif approach_axis.lower() == "x": - axis = 0 - elif approach_axis.lower() == "-x": - axis = 0 - approach_distance = -approach_distance - - above_goal = deepcopy(target_location) - above_goal[axis] += approach_distance - - self.logger.info(f"Starting place operation to target: {target_location}") - self.logger.debug("Moving to above goal position") - self.ur.movel(above_goal, self.acceleration, self.velocity) - - self.logger.debug("Moving to goal position") - self.ur.movel(target_location, self.acceleration, self.velocity) - - self.open_gripper() - - if self.resource_client and isinstance(target, LocationArgument): # Handle resources if configured - try: - popped_object, updated_resource = self.resource_client.pop(resource=self.gripper_resource_id) - self.resource_client.push(resource=target.resource_id, child=popped_object) - except Exception as e: - self.logger.error(f"Resource client error during place: {e}\n{traceback.format_exc()}") - - self.logger.debug("Moving back to above goal position") - self.ur.movel(above_goal, self.acceleration, self.velocity) - self.logger.info("Place operation completed successfully") - - except GripperOperationError: - raise - except Exception as e: - self.logger.error(f"Error during place operation: {e}\n{traceback.format_exc()}") - raise GripperOperationError(f"Place operation failed: {e}") # noqa - - def place_screw( - self, - target: Union[LocationArgument, list] = None, - screw_time: float = 9, - ) -> None: - """Handles the place screw request""" - # Move to the target location - - if isinstance(target, LocationArgument): - target_location = target.representation - elif isinstance(target, list): - target_location = target - - above_target = deepcopy(target_location) - above_target[2] += 0.03 - self.ur.movel(above_target, self.acceleration, self.velocity) - self.ur.movel(target_location, 0.2, 0.2) - - target_pose = [0, 0, 0.00021, 0, 0, 3.14] # Setting the screw drive motion - self.logger.info("Screwing down") - - self.ur.speedl_tool( - target_pose, 2, screw_time - ) # This will perform screw driving motion for defined number of seconds - sleep(screw_time) - self.logger.info("Screw drive motion completed") - - self.ur.translate_tool([0, 0, -0.03], 0.5, 0.5) - - def remove_cap( - self, - home: Union[LocationArgument, list] = None, - source: Union[LocationArgument, list] = None, - target: Union[LocationArgument, list] = None, - ) -> None: - """Handles the remove cap request""" - self.open_gripper() - if isinstance(source, LocationArgument): - source_location = source.representation - elif isinstance(source, list): - source_location = source - - above_goal = deepcopy(source_location) - above_goal[2] += 0.06 - self.ur.movel(above_goal, self.acceleration, self.velocity) - self.ur.movel(source_location, 0.2, 0.2) - - self.close_gripper() - - if self.resource_client and isinstance(source, LocationArgument): # Handle resources if configured - popped_object, updated_resource = self.resource_client.pop(resource=source.resource_id) - self.resource_client.push(resource=self.gripper_resource_id, child=popped_object) - - target_pose = [0, 0, -0.001, 0, 0, -3.14] # Setting the screw drive motion - self.logger.info("Removing cap") - screw_time = 7 - self.ur.speedl_tool( - target_pose, 2, screw_time - ) # This will perform screw driving motion for defined number of seconds - sleep(screw_time + 0.5) - self.ur.translate_tool([0, 0, -0.03], 0.5, 0.5) - - self.home_robot(home) - self.place(place_goal=target) - self.home_robot(home) - - def place_cap( - self, - home: Union[LocationArgument, list] = None, - source: Union[LocationArgument, list] = None, - target: Union[LocationArgument, list] = None, - ) -> None: - """Handles the replace cap request""" - - self.pick(pick_goal=source) - self.home_robot(home) - - if isinstance(target, LocationArgument): - target_location = target.representation - elif isinstance(target, list): - target_location = target - - above_goal = deepcopy(target_location) - above_goal[2] += 0.06 - self.ur.movel(above_goal, self.acceleration, self.velocity) - self.ur.movel(target_location, 0.1, 0.1) - - # self.close_gripper() - - target_pose = [0, 0, 0.0001, 0, 0, 2.10] # Setting the screw drive motion - self.logger.info("Placing cap") - screw_time = 6 - self.ur.speedl_tool( - target_pose, 2, screw_time - ) # This will perform screw driving motion for defined number of seconds - sleep(screw_time) - - self.open_gripper() - - if self.resource_client and isinstance(target, LocationArgument): # Handle resources if configured - popped_object, updated_resource = self.resource_client.pop(resource=self.gripper_resource_id) - self.resource_client.push(resource=target.resource_id, child=popped_object) - - self.ur.translate_tool([0, 0, -0.03], 0.5, 0.5) - self.home_robot(home) - - def flip_object( - self, - target: Union[LocationArgument, list] = None, - approach_axis: str = None, - ) -> None: - """Flips the object at the target location""" - - self.pick(pick_goal=target, approach_axis=approach_axis) - - cur_j = self.ur.getj() - rotate_j = cur_j - rotate_j[5] += radians(180) - self.ur.movej(rotate_j, 0.6, 0.6) - - cur_l = self.ur.getl() - target[3] = cur_l[3] - target[4] = cur_l[4] - target[5] = cur_l[5] - - self.place(place_goal=target, approach_axis=approach_axis) - - def transfer( - self, - home: Union[LocationArgument, list] = None, - source: Union[LocationArgument, list] = None, - target: Union[LocationArgument, list] = None, - source_approach_axis: str = None, - target_approach_axis: str = None, - source_approach_distance: float = None, - target_approach_distance: float = None, - ) -> None: - """Handles the transfer request""" - try: - self.logger.info("Starting transfer operation") - self.pick( - source=source, - approach_axis=source_approach_axis, - approach_distance=source_approach_distance, - ) - self.logger.info("Pick completed") - - self.home_robot(home=home) - - self.place( - target=target, - approach_axis=target_approach_axis, - approach_distance=target_approach_distance, - ) - self.logger.info("Place completed") - - except Exception as e: - self.logger.error(f"Error during transfer operation: {e}\n{traceback.format_exc()}") - raise - - def screw_transfer( - self, - home: Union[LocationArgument, list] = None, - target: Union[LocationArgument, list] = None, - screwdriver_loc: Union[LocationArgument, list] = None, - screw_loc: Union[LocationArgument, list] = None, - screw_time: float = 9, - ) -> None: - """Handles the transfer request""" - - self.pick( - pick_goal=screwdriver_loc, - ) # Pick up the screwdriver bit - self.home_robot(home=home) # Move back to home position - self.pick_screw(home=home, screw_loc=screw_loc) # Pick up the screw - self.place_screw(home=home, target=target, screw_time=screw_time) # Drive the screwdriving motion - self.home_robot(home=home) # Move back to home position - self.place(place_goal=screwdriver_loc) # Place the screwdriver bit - self.home_robot(home=home) - - -class VacuumGripperController: - """Robotiq Vacuum Gripper Controller""" - - def __init__( - self, - IP: str = "146.137.240.38", - PORT: int = 29999, - gripper: bool = False, - ): - """Constructor for VacummGripperController""" - super().__init__(IP=IP, PORT=PORT) diff --git a/src/ur_interface/ur_tools/grippers/abstract_gripper_interfaces.py b/src/ur_interface/ur_tools/grippers/abstract_gripper_interfaces.py new file mode 100644 index 0000000..1e02527 --- /dev/null +++ b/src/ur_interface/ur_tools/grippers/abstract_gripper_interfaces.py @@ -0,0 +1,23 @@ +"""Abstract gripper interface definitions.""" + +from abc import ABC, abstractmethod + + +class Gripper(ABC): + """Abstract gripper interface.""" + + @abstractmethod + def activate(self): + """activates the gripper""" + + +class FingerGripper(Gripper): + """Abstract gripper interface.""" + + @abstractmethod + def open(self, pose: float, speed: float, force: float): + """Open the gripper to a specified pose with given speed and force.""" + + @abstractmethod + def close(self, pose: float, speed: float, force: float): + """Close the gripper to a specified pose with given speed and force.""" diff --git a/src/ur_interface/ur_tools/robotiq_gripper_driver.py b/src/ur_interface/ur_tools/grippers/robotiq_2_finger_gripper_interface.py similarity index 89% rename from src/ur_interface/ur_tools/robotiq_gripper_driver.py rename to src/ur_interface/ur_tools/grippers/robotiq_2_finger_gripper_interface.py index 141a387..e712de4 100644 --- a/src/ur_interface/ur_tools/robotiq_gripper_driver.py +++ b/src/ur_interface/ur_tools/grippers/robotiq_2_finger_gripper_interface.py @@ -6,12 +6,15 @@ from enum import Enum from typing import OrderedDict, Tuple, Union +from ur_interface.ur_tools.grippers.abstract_gripper_interfaces import FingerGripper -class RobotiqGripper: + +class Robotiq2FingerGripper(FingerGripper): """ Communicates with the gripper directly, via socket with string commands, leveraging string names for variables. """ + tool_params = {"baud_rate": 115200, "parity": 0, "stop_bits": 1, "rx_idle_chars": 1.5, "tx_idle_chars": 3.5} # WRITE VARIABLES (CAN ALSO READ) ACT = "ACT" # act : activate (1 while activated, can be reset to clear fault status) GTO = "GTO" # gto : go to (will perform go to with the actions set in pos, for, spe) @@ -44,8 +47,11 @@ class ObjectStatus(Enum): STOPPED_INNER_OBJECT = 2 AT_DEST = 3 - def __init__(self): + def __init__(self, hostname: str, port: int = 63352, socket_timeout: float = 60.0): """Constructor.""" + self.hostname = hostname + self.port = port + self.socket_timeout = socket_timeout self.socket = None self.command_lock = threading.Lock() self._min_position = 0 @@ -54,6 +60,9 @@ def __init__(self): self._max_speed = 255 self._min_force = 0 self._max_force = 255 + self.speed = 255 + self.force = 255 + self.connect() def __del__(self): """Destructor.""" @@ -62,9 +71,6 @@ def __del__(self): def connect( self, - hostname: str, - port: int, - socket_timeout: float = 60.0, ) -> None: """Connects to a gripper at the given address. :param hostname: Hostname or ip. @@ -73,8 +79,8 @@ def connect( """ self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - self.socket.connect((hostname, port)) - self.socket.settimeout(socket_timeout) + self.socket.connect((self.hostname, self.port)) + self.socket.settimeout(self.socket_timeout) def disconnect(self) -> None: """Closes the connection with the gripper.""" @@ -200,7 +206,7 @@ def rq_activate_and_wait(gripper_socket="1"): def is_active(self): """Returns whether the gripper is active.""" status = self._get_var(self.STA) - return RobotiqGripper.GripperStatus(status) == RobotiqGripper.GripperStatus.ACTIVE + return self.GripperStatus(status) == self.GripperStatus.ACTIVE def get_min_position(self) -> int: """Returns the minimum position the gripper can reach (open position).""" @@ -236,19 +242,19 @@ def auto_calibrate(self, log: bool = True) -> None: """ # first try to open in case we are holding an object (position, status) = self.move_and_wait_for_pos(self.get_open_position(), 64, 1) - if RobotiqGripper.ObjectStatus(status) != RobotiqGripper.ObjectStatus.AT_DEST: + if self.ObjectStatus(status) != self.ObjectStatus.AT_DEST: raise RuntimeError(f"Calibration failed opening to start: {str(status)}") # try to close as far as possible, and record the number (position, status) = self.move_and_wait_for_pos(self.get_closed_position(), 64, 1) - if RobotiqGripper.ObjectStatus(status) != RobotiqGripper.ObjectStatus.AT_DEST: + if self.ObjectStatus(status) != self.ObjectStatus.AT_DEST: raise RuntimeError(f"Calibration failed because of an object: {str(status)}") assert position <= self._max_position self._max_position = position # try to open as far as possible, and record the number (position, status) = self.move_and_wait_for_pos(self.get_open_position(), 64, 1) - if RobotiqGripper.ObjectStatus(status) != RobotiqGripper.ObjectStatus.AT_DEST: + if self.ObjectStatus(status) != self.ObjectStatus.AT_DEST: raise RuntimeError(f"Calibration failed because of an object: {str(status)}") assert position >= self._min_position self._min_position = position @@ -303,10 +309,32 @@ def move_and_wait_for_pos(self, position: int, speed: int, force: int) -> Tuple[ # wait until not moving cur_obj = self._get_var(self.OBJ) - while RobotiqGripper.ObjectStatus(cur_obj) == RobotiqGripper.ObjectStatus.MOVING: + while self.ObjectStatus(cur_obj) == self.ObjectStatus.MOVING: cur_obj = self._get_var(self.OBJ) # report the actual position and the object status final_pos = self._get_var(self.POS) final_obj = cur_obj - return final_pos, RobotiqGripper.ObjectStatus(final_obj) + return final_pos, self.ObjectStatus(final_obj) + + def open(self): + """open the gripper""" + self.move_and_wait_for_pos( + 0, + self.speed, + self.force, + ) + time.sleep(0.5) + + def close(self): + """close the gripper""" + self.move_and_wait_for_pos(255, self.speed, self.force) + time.sleep(0.5) + + def set_speed(self, speed: int): + """set gripper speed""" + self.speed = speed + + def set_force(self, force: int): + """set gripper force""" + self.force = force diff --git a/src/ur_interface/ur_tools/interpreter_socket.py b/src/ur_interface/ur_tools/interpreter_socket.py deleted file mode 100644 index 1e31de0..0000000 --- a/src/ur_interface/ur_tools/interpreter_socket.py +++ /dev/null @@ -1,126 +0,0 @@ -"""Interpreter Socket to create remote communication with Interpreter URP generator that runs on UR polyscope""" - -import logging -import re -import socket -from time import sleep - -UR_INTERPRETER_SOCKET = 30020 - - -class InterpreterSocket: - """This socket creates a remote interface with the interpreter urp program generator to create urp programs on the polyscope remotely""" - - log = logging.getLogger("interpreter.InterpreterHelper") - STATE_REPLY_PATTERN = re.compile(r"(\w+):\W+(\d+)?") - - def __init__( - self, - hostname: str = None, - port: int = UR_INTERPRETER_SOCKET, - timeout: float = 2.0, - ) -> None: - """Constructor for the InterpreterSocket class. - :param hostname: Hostname or ip. - :param port: Port. - :param socket_timeout: Timeout for blocking socket operations. - """ - self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - self.hostname = hostname - self.port = port - self.timeout = timeout - self.response = None - - def connect(self) -> None: - """ - Connects to a tool at the given address. - """ - try: - self.socket.connect((self.hostname, self.port)) - self.socket.settimeout(self.timeout) - except socket.error as exc: - self.log.error(f"Interpreter Socket Error = {exc}") - raise exc - - def disconnect(self) -> None: - """Closes the connection with the Interpreter socket.""" - self.socket.close() - - def get_reply(self): - """ - read one line from the socket - :return: text until new line - """ - collected = b"" - while True: - part = self.socket.recv(4096) - if part != b"\n": - collected += part - elif part == b"\n": - break - self.response = collected.decode("utf-8") - - def execute_command(self, command) -> int: - """ - Send single line command to interpreter mode, and wait for reply - :param command: - :return: ack, or status id - """ - self.log.debug(f"Command: '{command}'") - if not command.endswith("\n"): - command += "\n" - - self.socket.send(command.encode("utf-8")) - self.get_reply() - self.log.debug(f"Reply: '{self.response}'") - # parse reply, raise exception if command is discarded - reply = self.STATE_REPLY_PATTERN.match(self.response) - if reply.group(1) == "discard": - raise Exception( - "Interpreter discarded message", - self.response, - ) - return int(reply.group(2)) - - def clear(self): - """Clear the urp programs""" - return self.execute_command("clear_interpreter()") - - def skip(self): - """Skips the buffer""" - return self.execute_command("skipbuffer") - - def abort_move(self): - """Abort""" - return self.execute_command("abort") - - def get_last_interpreted_id(self): - """Gets the last interpreted ID""" - return self.execute_command("statelastinterpreted") - - def get_last_executed_id(self): - """Gets the last executed ID""" - return self.execute_command("statelastexecuted") - - def get_last_cleared_id(self): - """Get the last cleared ID""" - return self.execute_command("statelastcleared") - - def get_unexecuted_count(self): - """Get unexecuted line count""" - return self.execute_command("stateunexecuted") - - def end_interpreter(self): - """End interpreter program""" - return self.execute_command("end_interpreter()") - - -if __name__ == "__main__": - """Tests""" - tool = InterpreterSocket(ip="164.54.116.129") - tool.connect() - # tool.execute_command("rq_screw_turn(1,1,3600,100,False,9)") - # tool.execute_command("") - # sleep(10) - # tool.execute_command("rq_screw_turn(1,1,3600,250,False,9)") - sleep(5) diff --git a/src/ur_interface/ur_tools/ot_pipette_controller.py b/src/ur_interface/ur_tools/ot_pipette_controller.py deleted file mode 100644 index 23b79d4..0000000 --- a/src/ur_interface/ur_tools/ot_pipette_controller.py +++ /dev/null @@ -1,536 +0,0 @@ -"""OT Pipette Controller Interface""" - -from time import sleep - -import epics - - -class OTPipetteController: - """A class to control the OT pipette over tbe EPICS PVs""" - - def __init__(self, pipette_pv: str = None, ur_connection=None): - """ - Initializes the OTPipetteController class to controll the OpenTron pipettes. - - Parameters: - - pipette_pv (str): The EPICS process variable (PV) for the pipette. - - ur_connection: The connection object for the Universal Robot (UR) robot. - """ - - self.pv = pipette_pv - - if not ur_connection: - raise Exception("UR connection is not established") - else: - self.ur = ur_connection - - self.acceleration = 0.5 - self.velocity = 0.2 - self.speed_ms = 0.750 - self.speed_rads = 0.750 - self.accel_mss = 1.200 - self.accel_radss = 1.200 - self.blend_radius_m = 0.001 - self.ref_frame = [0, 0, 0, 0, 0, 0] - - self.pipette_drop_tip_value = -8 - self.pipette_aspirate_value = 2.0 - self.pipette_dispense_value = -2.0 - self.droplet_value = 0.3 - - self.pipette_loc = [ - -0.30710397664568057, - 0.2223363316295067, - 0.25346649921490616, - 0.9780579194931717, - -1.3456500374612195, - -1.5122814896417478, - ] - self.pipette_loc_J = [ - 2.8711442947387695, - -1.8251310787596644, - -1.5156354904174805, - -1.3721376222423096, - -4.720762554799215, - -3.0886977354632776, - ] - self.pipette_approach = [ - -0.30710468347240427, - 0.22234393663902577, - 0.2793166289891617, - 0.977973715529265, - -1.3455795650125528, - -1.512392593568845, - ] - self.pipette_approach_J = [ - 2.8711442947387695, - -1.8102451763548792, - -1.412275791168213, - -1.4902585309794922, - -4.720990244542257, - -3.088721577321188, - ] - self.pipette_above = [ - -0.3075719688934094, - 0.2227307810713913, - 0.40515454739546075, - 0.9815940238325527, - -1.3416684284127856, - -1.504904936327573, - ] - self.pipette_above_J = [ - 2.869802236557007, - -1.9749981365599574, - -0.5613865852355957, - -2.1772977314391078, - -4.720307175313131, - -3.0981438795672815, - ] - self.tip1_loc = [ - 0.049076405377552826, - 0.35130426249264163, - 0.063, - 0.9759108742683295, - -1.3350220046082053, - -1.5092226077826993, - ] - self.tip1_approach = [ - 0.049076095756735466, - 0.3513032390285145, - 0.083, - 0.9758916159413838, - -1.3350252553821587, - -1.5092057412143818, - ] - self.tip1_above = [ - 0.04908221782054774, - 0.3513003341332178, - 0.138, - 0.9758574103691817, - -1.3350463108315163, - -1.5091909291569083, - ] - self.tip2_loc = [ - 0.04909177440821851, - 0.3411316353820866, - 0.0628, - 0.977119433532159, - -1.3337829736507698, - -1.5108373189678133, - ] - self.tip2_approach = [ - 0.04909177440821851, - 0.3411316353820866, - 0.083, - 0.977119433532159, - -1.3337829736507698, - -1.5108373189678133, - ] - self.tip2_above = [ - 0.04909177440821851, - 0.3411316353820866, - 0.138, - 0.977119433532159, - -1.3337829736507698, - -1.5108373189678133, - ] - self.sample1 = [ - 0.15220619381604186, - 0.21043816573205595, - 0.09618091909170277, - 1.444826407763332, - -0.2548060433102738, - -0.31289353129621067, - ] - self.sample1_above = [ - 0.15220723461648447, - 0.2104311001071656, - 0.14402782259610025, - 1.4448359749910735, - -0.2548206714588542, - -0.31295915781137074, - ] - self.sample2_above = [ - 0.15279755520703087, - 0.18939793717407497, - 0.14402267332894347, - 1.444821393022025, - -0.25485812796155616, - -0.3128929822914916, - ] - self.sample2 = [ - 0.15186061464767017, - 0.18822197623964088, - 0.09490910394912143, - 1.4440966224799245, - -0.255613147568461, - -0.3122426586441542, - ] - self.empty_tube = [ - 0.15203368788019977, - 0.16531582069324421, - 0.12185568609417977, - 1.4402850302548993, - -0.2846256403901101, - -0.3468228184833902, - ] - self.empty_tube_above = [ - 0.15203001904780783, - 0.16531236663764431, - 0.14222620538915642, - 1.4402337440190125, - -0.2846450307479814, - -0.346876615018759, - ] - self.well1 = [ - 0.12772478460859046, - 0.21370236710062357, - 0.08390608100945282, - 1.4380130231592743, - -0.2414629895555231, - -0.2954608172533908, - ] - self.well1_above = [ - 0.12773445855037924, - 0.21371308008717516, - 0.1271232135439438, - 1.4380596200664426, - -0.24151536289689018, - -0.2954919320386042, - ] - self.trash_bin_above = [ - 0.187412530306272, - 0.2868009561100828, - 0.12712991727750073, - 1.438076830279249, - -0.2414934112798892, - -0.2954944172453427, - ] - self.trash_bin = [ - 0.1874179391982658, - 0.2867862635600429, - 0.013156853887081085, - 1.438022625162957, - -0.24148065729851562, - -0.2954808450568972, - ] - - def connect_pipette(self): - """ - Connect pipette - """ - - try: - # Establishing a connection with the pipette on EPICS - self.pipette = epics.PV(self.pv) - - except Exception as err: - print("Pipette error: ", err) - - else: - print("Pipette is connected") - - def disconnect_pipette(self): - """ - Disconnect pipette - """ - - try: - # Closing the connection with the pipette on EPICS - self.pipette.disconnect() - - except Exception as err: - print("Pipette error: ", err) - - else: - print("Pipette is disconnected") - - def move_pipette_dock(self): - """ - Description: Moves the robot to the doscking location and then picks up the pipette. - """ - print("Picking up the pipette...") - speed_ms = 1.00 - - print("Picking up the pipette...") - sleep(1) - self.ur.movel( - self.pipette_above, - self.accel_mss, - speed_ms, - 0, - 0, - ) - sleep(2) - self.ur.movel( - self.pipette_approach, - self.accel_mss, - speed_ms, - 0, - 0, - ) - speed_ms = 0.01 - sleep(1) - self.ur.movel(self.pipette_loc, self.accel_mss, speed_ms, 0, 0) - sleep(5) - # LOCK THE TOOL CHANGER TO ATTACH THE PIPETTE HERE - - def lift_pipette_on_dock(self): - """ - Description: Moves the robot to the doscking location and then picks up the pipette. - """ - sleep(5.0) - speed_ms = 0.1 - self.ur.movel( - self.pipette_approach, - self.accel_mss, - speed_ms, - 0, - 0, - ) - sleep(1) - self.ur.movel( - self.pipette_above, - self.accel_mss, - speed_ms, - 0, - 0, - ) - sleep(2) - print("Pipette successfully picked up") - - def pick_tip(self, tip_loc, x=0, y=0): - """ - Description: Picks up a new tip from the first location on the pipette bin. - """ - - tip_approach = tip_loc - tip_approach[2] += 0.02 - tip_above = tip_loc - tip_above[2] += 0.075 - - print("Picking up the first pipette tip...") - speed_ms = 0.100 - - self.ur.movel( - tip_above, - self.accel_radss, - self.speed_rads, - 0, - 0, - ) - sleep(2) - speed_ms = 0.01 - self.ur.movel( - tip_approach, - self.accel_radss, - self.speed_rads, - 0, - 0, - ) - sleep(2) - self.ur.movel(tip_loc, self.accel_mss, speed_ms, 0, 0) - sleep(3) - self.ur.movel(tip_approach, self.accel_mss, speed_ms, 0, 0) - sleep(2) - speed_ms = 0.1 - self.ur.movel(tip_above, self.accel_mss, speed_ms, 0, 0) - sleep(2) - print("Pipette tip successfully picked up") - - def transfer_sample(self, sample_loc, well_loc): - """ - Description: - - Makes a new sample on the 96 well plate. - - Mixes to liquits in a single well and uses a new pipette tip for each liquid. - - In order to mix the liquids together, pipette performs aspirate and dispense operation multiple times in the well that contains both the liquids. - """ - print("Making a sample using two liquids...") - - # MOVE TO THE FIRT SAMPLE LOCATION - speed_ms = 0.1 - - sample_above = sample_loc - sample_above[2] += 0.05 - well_above = well_loc - well_above[2] += 0.05 - - self.ur.movel( - sample_above, - self.accel_mss, - self.speed_ms, - 0, - 0, - ) - sleep(2) - self.ur.movel(sample_loc, self.accel_mss, speed_ms, 0, 0) - sleep(2) - - # ASPIRATE FIRST SAMPLE - self.aspirate_pipette() - self.ur.movel(sample_above, self.accel_mss, speed_ms, 0, 0) - sleep(1) - - # MOVE TO THE 1ST WELL - self.ur.movel(well_above, self.accel_mss, speed_ms, 0, 0) - sleep(1) - self.ur.movel(well_loc, self.accel_mss, speed_ms, 0, 0) - sleep(1) - - # DISPENSE FIRST SAMPLE INTO FIRST WELL - self.dispense_pipette() - self.ur.movel(well_above, self.accel_mss, speed_ms, 0, 0) - sleep(1) - - def mix_samples(self, well_loc): - """Mix samples - - Args - well_loc: Well location - """ - well_above = well_loc - well_above[2] += 0.05 - - # MOVE TO THE 1ST WELL - self.ur.movel(well_above, self.accel_mss, self.speed_ms, 0, 0) - sleep(1) - self.ur.movel(well_loc, self.accel_mss, self.speed_ms, 0, 0) - sleep(1) - - # MIX SAMPLE - for i in range(3): # noqa - self.aspirate_pipette() - self.dispense_pipette() - - # Aspirate all the liquid - self.aspirate_pipette() - self.aspirate_pipette() - - self.ur.movel(well_above, self.accel_mss, self.speed_ms, 0, 0) - sleep(1) - print("Sample is prepared") - - def aspirate_pipette(self): - """ - Description: - - Drives pipette to aspirate liquid. - - Number of motor steps to aspirate liquid is stored in "self.pipette_aspirate_value". - - Pipette is controlled by pyepics PV commands. - """ - print("Aspirating the sample...") - current_value = self.pipette.get() - self.pipette.put(float(current_value) + self.pipette_aspirate_value) - sleep(1) - - def dispense_pipette(self): - """ - Description: - - Drives pipette to dispense liquid. - - Number of motor steps to dispense liquid is stored in "self.pipette_dispense_value". - - Pipette is controlled by pyepics PV commands. - """ - print("Dispensing sample") - current_value = self.pipette.get() - self.pipette.put(float(current_value) + self.pipette_dispense_value) - sleep(1) - - def create_droplet(self): - """ - Description: - - Drives pipette to create a droplet. - - Number of motor steps to create a droplet is stored in "self.droplet_value". - - Pipette is controlled by pyepics PV commands. - """ - print("Creating a droplet...") - current_value = self.pipette.get() - self.pipette.put(float(current_value) - self.droplet_value) - sleep(10) - - def retrieve_droplet(self): - """ - Description: - - Retrieves the droplet back into the pipette tip. - - Number of motor steps to retrieve a droplet is stored in "self.droplet_value". - - Pipette is controlled by pyepics PV commands. - """ - print("Retrieving droplet...") - current_value = self.pipette.get() - self.pipette.put(float(current_value) + self.droplet_value + 0.5) - sleep(1) - - def drop_tip_to_trash(self): - """ - Description: Drops the pipette tip by driving the pipette all the way to the lowest point. - """ - - print("Droping tip to the trash bin...") - # Move to the trash bin location - self.ur.movel( - self.trash_bin_above, - self.accel_mss, - self.speed_ms, - 0, - 0, - ) - sleep(2) - self.ur.movel( - self.trash_bin, - self.accel_mss, - self.speed_ms, - 0, - 0, - ) - sleep(2) - self.eject_tip() - sleep(1) - self.ur.movel( - self.trash_bin_above, - self.accel_mss, - self.speed_ms, - 0, - 0, - ) - sleep(2) - - def eject_tip(self): - """ - Description: Ejects the pipette tip - """ - print("Ejecting the tip") - self.pipette.put(self.pipette_drop_tip_value) - sleep(2) - self.pipette.put(0) - sleep(2) - - def empty_tip(self): - """ - Description: Dispenses all the liquid inside pipette tip. - """ - print("Empting tip...") - speed_ms = 0.5 - # Moving the robot to the empty tube location - self.ur.movel( - self.empty_tube_above, - self.accel_mss, - self.speed_ms, - 0, - 0, - ) - sleep(2) - speed_ms = 0.1 - self.ur.movel(self.empty_tube, self.accel_mss, speed_ms, 0, 0) - sleep(2) - - # Drive the pipette three times to dispense all the liquid inside the pipette tip. - for i in range(3): # noqa - self.dispense_pipette() - sleep(1) - - self.ur.movel( - self.empty_tube_above, - self.accel_mss, - speed_ms, - 0, - 0, - ) - sleep(1) diff --git a/src/ur_interface/ur_tools/pipette_driver.py b/src/ur_interface/ur_tools/pipette_driver.py deleted file mode 100644 index 31946a9..0000000 --- a/src/ur_interface/ur_tools/pipette_driver.py +++ /dev/null @@ -1,808 +0,0 @@ -""" -This is a python code to control the Z-series pump of Tricontinent. -It assumes two things: -1. the pump is connected to the Tool I/O of a UR robot, which requires wiring of 4 pins of the Z-pump control board. -Example, when a Lumberg RKMV 8-354 is used, its gray, red, brown, and white cables should go 24V, GND, RS485B, and RS485A pins. -According to a UR3 manual, gray for Power, red for GND, brown for Analog input 3, and white for Analog input 2. -2. a URcap for RS485 daemon is required. Download and install from https://forum.universal-robots.com/t/establish-rs485-connection-with-tool/14821/5 - -Author: Byeongdu Lee (Argonne National Laboratory) -Date: July, 2023 -""" - -import socket -import threading -import time -from typing import OrderedDict, Union - -# HOST = 'ur5-12idc.xray.aps.anl.gov' -HOST = "164.54.116.129" -""" -# set commands -Initialization Commands -V (5-6000) Set top speed in half steps per second. Default = 1400 -v (0-1000) Set start speed in half steps per second. Default = 0 -c (50-2700) Set stop speed in half steps per second. Default = 900 -S (0-40) Set top speed using speed codes. Default = 11 -L (1-20) Set acceleration factor (accel="L"*2.5 kHz/sec.) Default = 14 -m (0-100) Sets the Motor Run current in a % of maximum (500mA peak). For example, m50R will set the run -current to 50% of its maximum (250mA). Similar to the [u2] command, only this setting will be lost when -the power is cycled, or it is volatile. Whereas the [u2] is non-volatile. - -h (0-100) Sets the Motor Hold current in a % of maximum (500mA peak). For example, h10R will set the hold -current to 10% of its maximum (50mA). Similar to the [u1] command, only this setting will be lost when -the power is cycled, or it is volatile. Whereas the [u1] is non-volatile. - -N (0-1) N = 0, all motor positions are in half steps; N=1, positions are in micro- steps, 8 micro-steps per half-step. -Default N = 0 or half-step mode. - -K (0-31) Sets number of backlash steps. Default K = 0. -k (0-80) Syringe dead volume. After initialization, the plunger will move this many half-steps to minimize the dead -volume. Default k = 0. - -u (n_X) Will load pump configuration and calibration info into the internal EEPROM. Note, these parameters are -only read on power up. Thus they will only take effect when the power is cycled. Note this command, -unlike the Set commands, does not require an [R] to execute. -1. (1_XXX) Motor holding current, 0 –100% (100% = 500mA peak) -2. (2_XXX) Motor running current, 0 –100% -3. (3_XXX) Max home steps in 100 half-step increments (1-250) -4. (4_XXX) Max home speed in 100 half-steps/sec increments (1-100) -5. (5_XXX) Homing back-steps in 100 half-steps increments(1-250) -6. (6_XXX) Default max V in 100 half-steps/sec increments (1-100) -7. (7_XXX) Max plunger stroke in 100 half-step increments (1–250) -8. (8_XXX) Home position at top, X= 1, at bottom X= 0 -9. (9_XXX) Number of user settable outputs(0–4) -10. (10_X) No homing opto, X=0. Homing opto installed, X=1 -11. (11_X) Stall guard level for no-opto homing (1-7) -12. (12_X) Solenoid daughter installed, X=1, not installed X=0 -13. (13_X) CAN Bus option installed, X=1, not installed X=0 -14. (14_X) Number of backlash steps -15. (15_X) Motor winding for LT, X=1, for Z-Pump X=0 -16. (16_X) Home sensor polarity low = blocked, X=1, low = unblocked X=0 -17. (17_X) Self test mode string - -z (0-1600) Sets current position and initializes the plunger to the value defined by the operand. -(0-12,800 in micro-step mode) - - -Movement Commands -Note: The limits below assume 1600 half-steps per stroke. The limits for micro-step mode [N =1] will always be 8X that of -the half-step [N=0] mode. -Report Commands -A (0-1600) Move motor to absolute position (0-12,800 in micro-step mode). -a (0-1600) Same as [A], but will give a non-busy status code. -P (0-1600) Move motor relative number of steps in the aspirate direction (0-12,800 in micro-step mode). -p (0-1600) Same as [P], but will give a non-busy status code. -D (0-1600) Move motor relative number of steps in the dispense direction (0-12,800 in micro-step mode). -d (0-1600) Same as [D], but will give a non-busy status code. -Q Returns the status character. (refer to DT Protocol, Answer Block) -? or ?0 Returns current absolute position. -?1 Returns start speed. -?2 Returns top speed. -?3 Returns stop speed. -?7 Reports max homing steps. -?8 Reports homing speed. -?9 Reports homing back steps. -?10 Reports syringe dead volume. -?11 Reports backlash steps. -?24 Reports syringe dead volume. -?25 Reports motor hold current. -?26 Reports motor run current. -? (30-44) Reports user program strings loaded into external or user EEPROM. -?30 reports string 0, ?31 string 1, and so on. -& Returns the firmware revision and date. -F Reports command buffer status. If the buffer is empty, the pump returns status code -""" - - -## Exception handling.... -class CommunicationException(Exception): - """Commincation Error Exception""" - - pass - - -class PipetError(Exception): - """Pipette Error Exception""" - - pass - - -class PipetteTimeout(Exception): - """Pipette Timeout Exception""" - - pass - - -class PipetSocketError(Exception): - """Pipette Socker Error Exception""" - - pass - - -class PipetteDriver: - """Tricontinent Pipette Driver. This object creates a socket connection with RS485 URCap that is running on UR polyscope to send remote commands""" - - # host = 'robot_ip' - # port = 54321# Remains the same, because it is specified as default port in URCaps code - # s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - # s.connect((host, port)) - # s.sendall(b'Hello rs485 port from ash') - # data = s.recv(1024) - # s.close() - # print('Received', repr(data)) - - ENCODING = "UTF-8" # ASCII and UTF-8 both seem to work - NoError_NotBusy = "`" - NoError_Busy = "@" - # InitializationError_NotBusy = "a" - # InitializationError_Busy = "A" - # InvalidCommand_NotBusy = "b" - # InvalidCommand_Busy = "B" - # InvalidOperand_NotBusy = "c" - # InvalidOperand_Busy = "C" - # DeviceNotInitialized_NotBusy = "g" - # DeviceNotInitialized_Busy = "G" - # CommandOverflow_NotBusy = "o" - # CommandOverflow_Busy = "O" - - def __init__(self): - """Constructor.""" - self.socket = None - self.command_lock = threading.Lock() - self._min_position = 0 - self._max_position = 1600 - self._min_start_speed = 0 - self._max_start_speed = 1000 - self._min_top_speed = 5 - self._max_top_speed = 6000 - self._min_stop_speed = 50 - self._max_stop_speed = 2700 - self._min_backlash_step = 0 - self._max_backlash_step = 31 - self._min_deadvolume = 0 - self._max_deavvolume = 80 - self.volume = 200 # 200uL in full volume. - # for this volume resolution is 0.1905 uL - self._comm_setting = { - "baud_rate": 9600, - "parity": 0, - "stop_bits": 1, - "rx_idle_chars": 1.5, - "tx_idle_chars": 3.5, - } - self._step = 0 - - def get_step(self): - """Get step""" - try: - pos = self._get_var("?0") - return pos - except PipetSocketError: - pos = -1 - timeout = 10 - cnt = 1 - while (pos < 0) and (cnt < timeout): - try: - pos = self._get_var("?0", trial=cnt) - except PipetSocketError: - pos = -1 - time.sleep(0.1 * cnt) - if cnt > 1: - self.disconnect() - # print(f"Pipet: get_step tried {cnt} times.") - cnt = cnt + 1 - self.connect() - self._step = pos - if cnt == timeout: - raise CommunicationException("Pipet Communication Timeout.") - return pos - - def get_step_percent(self): - """Get step percentage""" - pos = self.get_step() - return self._convert_step_to_percent(pos) - - def get_volume(self): - """Get valume - - Returns: Volume - """ - pos = self.get_step() - return self._convert_step_to_volume(pos) - - def get_speed_start(self): - """Get speed start""" - return self._get_var("?1") - - def get_speed_start_percent(self): - """Get speed start percentage""" - return self.get_speed_start() / self._max_start_speed * 100 - - def get_speed(self): - """Get speed""" - return self._get_var("?2") - - def get_speed_percent(self): - """Get speed percent""" - return self.get_speed() / self._max_top_speed * 100 - - def get_speed_stop(self): - """Get speed stop""" - return self._get_var("?3") - - def get_max_homing_steps(self): - """Get maximum homing steps""" - return self._get_var("?7") - - def get_speed_stop_percent(self): - """Get stop percentage""" - return self.get_speed_stop() / self._max_stop_speed * 100 - - def get_dead_volume(self): - """Get dead volume""" - return self._get_var("?10") - - def get_backlash(self): - """Get backlash""" - return self._get_var("?11") - - def get_dead_volume2(self): - """Get dead volume2""" - return self._get_var("?24") - - def get_hold_current(self): - """Get gold current""" - return self._get_var("?25") - - def get_run_current(self): - """Get run current""" - return self._get_var("?26") - - def get_parameter(self, parameter): - """Get parameter""" - return self._get_var(parameter, isfloat=False) - - def initialize(self): - """Initializes the pipette motor""" - self.send_command("z1600A0A10z0", wait=True) - time.sleep(10) - - def set_speed(self, start, top=100, stop=100): - """Sets the speed - Args - start: Start value - top: Top value - stop: Stop value - """ - # self.reconnect() - # time.sleep(1) - self._set_vars({"v": start, "V": top, "c": stop}) - - def set_motorcurrent(self, value=50): - """Sets motor current - Args - value: Current value - """ - self._set_var("m", value) - - def set_step(self, value): - """Sets the steps - Args - value: Step value - """ - self.send_command(f"z{value}", wait=True) - - def dispense(self, vol=0, percent=0, start=0, speed=0, stop=0): - """Dispense pipette - - Args - vol: Volume - percent: Percentage - start: Start step - speed: Speed of the pipette - stop: Stop step - """ - cvol = self.get_volume() - cpos = self._convert_vol_to_step(cvol) - # print("got the volume.") - if vol != 0: - pos = self._convert_vol_to_step(vol) - else: - pos = self._convert_percent_to_step(percent) - par = {"D": pos} - if cpos - pos < 0: - raise PipetError("Cannot dispense to negative volume.") - if start != 0: - par.update({"v": start}) - if speed != 0: - par.update({"V": speed}) - if stop != 0: - par.update({"c": stop}) - self._set_vars(par) - - newpos = self.get_step() - while newpos > cpos - pos: - time.sleep(0.1) - _pos = self.get_step() - if newpos == _pos: - break - else: - newpos = _pos - vol = self.get_volume() - print(f"Pipet is at {vol} uL position.") - - def aspirate(self, vol=0, percent=0, start=0, speed=0, stop=0): - """Aspirate pipette - - Args - vol: Volume - percent: Percentage - start: Start step - speed: Pipette speed - stop: Stop step - """ - cvol = self.get_volume() - cpos = self._convert_vol_to_step(cvol) - if vol != 0: - pos = self._convert_vol_to_step(vol) - else: - pos = self._convert_percent_to_step(percent) - if cpos + pos > self._max_position: - raise PipetError("Cannot aspirate more than the max.") - par = {"P": pos} - if start != 0: - par.update({"v": start}) - if speed != 0: - par.update({"V": speed}) - if stop != 0: - par.update({"c": stop}) - self._set_vars(par) - - newpos = self.get_step() - while newpos < cpos + pos: - time.sleep(0.1) - _pos = self.get_step() - if newpos == _pos: - break - else: - newpos = _pos - - # while (self.get_step() < cpos+pos): - # time.sleep(0.1) - - vol = self.get_volume() - print(f"Pipet is at {vol} uL position.") - - def _convert_vol_to_step(self, vol): # mL - """Convert the mL volume to steps to drive the pipette - - Args - vol: Volume in mL - """ - percent = vol / self.volume * 100 - return self._convert_percent_to_step(percent) - - def _convert_step_to_volume(self, value): - """Converts step value to valume to drive the pipette - - Args - value: Step value - """ - - vol = value / self._max_position * self.volume - return vol - - def _convert_step_to_percent(self, value): - """Converts step value to percentage to drive the pipette - - Args - value: Move value - """ - - percent = value / self._max_position * 100 - return percent - - def _convert_percent_to_step(self, percent): - """Converts percentage value to steps to drive the pipette - - Args - percent: Move percentage - """ - value = percent / 100 * self._max_position - return int(value) - - def stop(self): - """Stops the pipette""" - self.send_command("T") - - def move(self, newpos, relative: bool = True): - """Moves the pipette - - Args: - newpos: New pose - relative (bool): True/False - """ - if relative: - pos = self.get_step() - newpos = pos + newpos - return self._set_var("A", newpos) - - def connect( - self, - hostname: str = HOST, - port: int = 54321, - socket_timeout: float = 1, - ) -> None: - """Connects to a pipette at the given address""" - if hasattr(self, hostname): - con = (self.hostname, self.port) - else: - con = (hostname, port) - self.address = con - self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - self.socket.connect(con) - self.socket.settimeout(socket_timeout) - - def reconnect(self): - """Reconnect to the pipette""" - self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - self.socket.connect(self.address) - self.socket.settimeout(1) - # self.socket.connect(self.address) - - def disconnect(self) -> None: - """Closes the connection with the pipette""" - try: - self.socket.close() - except Exception as err: - print("Connection has never been made. Err: " + err) - - def var_test( - self, - var_dict: OrderedDict[str, Union[int, float]], - wait: bool = True, - ): - """Tests the variables given - - Args: - var_dict (OrderedDict): variable dictinoary - wait (bool): True/False - """ - for variable, value in var_dict.items(): - print(f"{variable}{str(value)}.") - - def _set_vars( - self, - var_dict: OrderedDict[str, Union[str, int, float]], - timeout=10, - wait=True, - ): - """set variables or parameters. - :param var_dict: Dictionary of variables to set (variable_name, value). - :return: True on successful reception of ack, false if no ack was received, indicating the set may not - have been effective. - """ - # construct unique command - cmd = "/1" - for variable, value in var_dict.items(): - if isinstance(value, str): - val = value - else: - val = str(value) - cmd += f"{variable}{val}" - cmd += "R\r" # R for execution and new line is required for the command to finish - # print("COMMAND:", cmd) - # atomic commands send/rcv - status, val, data = self.query(cmd) - # print(f"Data: {data}, Status: {status}, Value: {val}") - errcheck, notbusy = self._status_check(status) - # with self.command_lock: - # self.socket.sendall(cmd.encode(self.ENCODING)) - # time.sleep(0.1) - # ans = "" - # data = "" - # try: - # data = self.socket.recv(1024) - # # print(data, "Normal") - # except CommunicationException: - # data = "" - # except ConnectionAbortedError: - # self.connect() - # data = "" - # except socket.timeout: - # print("socket timeout in set_var") - # time.sleep(0.1) - # data = "" - # try: - # ans = self._get_answer(data) - # except: - # # print(data, "Decode error on line 347") - # pass - # if len(ans)>0: - # errcheck, notbusy = self._status_check(ans) - # else: - # wait = False - # errcheck = False - - t = time.time() - timeout = 5 - if wait: - while not notbusy: - time.sleep(0.1) - errcheck, notbusy = self.get_status() - if time.time() - t > timeout: - # print("get_status timeout in set_vars.") - break - return errcheck - # if noerror: - # while notbusy: - - def get_status(self): - """Get status of the pipette""" - cmd = "/1Q\r" - readdone = False - timeout = 10 - cnt = 1 - while (not readdone) and (cnt < timeout): - try: - status, _, _ = self.query(cmd) - errcheck, busycheck = self._status_check(status) - readdone = True - except CommunicationException: - time.sleep(0.1 * cnt) - except socket.timeout: - time.sleep(0.1 * cnt) - except ConnectionAbortedError: - self.connect() - time.sleep(0.1 * cnt) - except Exception as err: - self.connect() - time.sleep(0.1 * cnt) - print(err) - cnt = cnt + 1 - if cnt == timeout: - raise CommunicationException("Time out.") - return (errcheck, busycheck) - - def query(self, cmd, trial=10, timeofsleep=0.1): - """Query a command over the socket to execute it on the pipette - Args - cmd: Command to be sent - trial: Max number of tries to try running the command - timeofsleep: Sleep time of - """ - readdone = False - cnt = 1 - data = "" - status = "" - with self.command_lock: - while (readdone is False) and (cnt < trial): - try: - self.socket.sendall(cmd.encode(self.ENCODING)) - time.sleep(timeofsleep) - data = self._recv() - readdone = True - except CommunicationException: - time.sleep(0.1 * cnt) - except ConnectionAbortedError: - self.reconnect() - except PipetteTimeout as err: - print(err) - return "", "", data - except BrokenPipeError as err2: - print(err2) - self.reconnect() - except Exception as err: - print("Pipette query error:", err) - self.reconnect() - cnt = cnt + 1 - if cnt == trial: - raise PipetteTimeout - if len(data) > 0: - status, val = self._get_answer(data) - else: - val = "" - return status, val, data - - def _recv(self): - """Recieves a response message from the socket""" - data = b"" - k = b"/" - timeout = 5 - t = time.time() - while True: - try: - k = self.socket.recv(1) - if len(k) > 0: - if isinstance(k, bytes): - data += k - if k[0] == 10: - break - else: - break - if time.time() - t > timeout: - raise PipetteTimeout - except socket.timeout as time_err: - raise PipetteTimeout from time_err - return data - - def send_command(self, command, value="", timeout=10, wait=True): - """Sends a command over the socket""" - return self._set_var(command, value=value, timeout=timeout, wait=wait) - - def _set_var( - self, - variable: str, - value: Union[str, int, float], - timeout=10, - wait=True, - ): - """set a single variable. - :param variable: Variable to set. - :param value: Value to set for the variable. - :return: True on successful reception of ack, false if no ack was received, indicating the set may not - have been effective. - """ - return self._set_vars( - OrderedDict([(variable, value)]), - timeout=timeout, - wait=wait, - ) - - def _get_var(self, variable: str, trial=3, isfloat=True): - """Retrieve the value of a variable from the pipet, blocking until the - response is received or the socket times out. - :param variable: Name of the variable to retrieve. - :return: Value of the variable as integer. - """ - # atomic commands send/rcv - cmd = "/1" - cmd += f"{variable}" - cmd += "\r" - # readdone = False - # cnt = 1 - # data = "" - status, val, data = self.query(cmd) - resp, notbusy = self._status_check(status) - if resp is False: - raise PipetSocketError - if isfloat is False: - return val - try: - return float(val) - except ValueError as value_err: - # print(data, "Pipet: Return decode error. ans should be a number string.") - raise PipetSocketError from value_err - - def _get_answer(self, data): - if len(data) == 0: - raise CommunicationException - dt = data.split(b"\n") - if len(dt) == 1: - raise CommunicationException - # return should start from /x (id of the controller) and ends with \n - data = dt[len(dt) - 2] - data, _ = data.split(b"\x03") - if data[0] != 47: - raise CommunicationException - status = data[2] - status = chr(status) - if len(data) > 2: - value = data[3:] - value = value.decode(self.ENCODING) - else: - value = None - return status, value - - def _decode_answer(self, data): - if len(data) == 0: - raise CommunicationException - dt = data.split(b"\n") - if len(dt) == 1: - raise CommunicationException - # return should start from /x (id of the controller) and ends with \n - data = dt[len(dt) - 2][1:] - data, _ = data.split(b"\x03") - data = data.decode(self.ENCODING) - ans = data[2:] - return ans - - def _status_check(self, data): - resp = "" - notbusy = False - if len(data) == 0: - return (False, notbusy) - # raise CommunicationException - if data == "`": - # print('Not busy') - return ("", True) - if data == "@": - # print('Busy') - return ("", False) - if data.isupper(): - # isbusy = "Busy" - notbusy = True - else: - # isbusy = "NotBusy" - notbusy = False - data = data.lower() - if data == "a": - resp = "Initialization Error" - if data == "b": - resp = "Invalid Command" - if data == "c": - resp = "Invalid Operand" - if data == "g": - resp = "Device Not Initialized" - if data == "o": - resp = "Command Overflow" - if data == "i": - resp = "Plunger Overload" - if data == "h": - resp = "CAN Bus failure" - raise PipetError(resp) - - # try: - # print(resp, notbusy) - # except: - # print(data, "status_check") - # return (False, notbusy) - # NoError_NotBusy = "`" - # NoError_Busy = "@" - # InitializationError_NotBusy = "a" - # InitializationError_Busy = "A" - # InvalidCommand_NotBusy = "b" - # InvalidCommand_Busy = "B" - # InvalidOperand_NotBusy = "c" - # InvalidOperand_Busy = "C" - # DeviceNotInitialized_NotBusy = "g" - # DeviceNotInitialized_Busy = "G" - # CommandOverflow_NotBusy = "o" - # CommandOverflow_Busy = "O" - - @staticmethod - def _is_ack(data: str): - return data == b"ack" - - -if __name__ == "__main__": - for i in range(10): # noqa - a = PipetteDriver() - # a.connect(hostname="164.54.116.129") - a.connect(hostname="192.168.100.109") - # time.sleep(5) - a.initialize() - # time.sleep(15) - speed = 111 - # a.set_speed(start=speed, top=speed, stop=speed) - # time.sleep(5) - error = True - while error: - try: - a.set_speed(start=speed, top=speed, stop=speed) - except Exception as e: - print("Error in setting speed:", e) - else: - error = False - # time.sleep(10) - # time.sleep(5) - # a.aspirate(vol=40) - # a.dispense(vol=40) - # time.sleep(5) - # a.dispense(vol=30,start=100,speed=50) - # time.sleep(15) - - # time.sleep(5) - # print(a.get_speed_start(), a.get_speed_stop(),a.get_speed()) - # a.dispense(vol=25) - # time.sleep(5) - # print(a.get_step()) - # a.dispense(vol=2) - # cmd = '/1' - # if len(sys.argv)<2: - # cmd += 'z1600A0A10z0R' - # else: - # cmd += sys.argv[1] - # print(a.send_command("z1600A0A10z0R")) - a.disconnect() diff --git a/src/ur_interface/ur_tools/robotiq_screwdriver_driver.py b/src/ur_interface/ur_tools/robotiq_screwdriver_driver.py deleted file mode 100644 index 08cae2e..0000000 --- a/src/ur_interface/ur_tools/robotiq_screwdriver_driver.py +++ /dev/null @@ -1,348 +0,0 @@ -"""Robotiq Screwdriver Driver Class""" - -from .interpreter_socket import InterpreterSocket - - -class RobotiqScrewdriver: - """ - RobotiqScrewdriver is a class created for UR robots to utilize Robotiq Screwdriver tool. - This class creates and socket connection utilizing the InterpreterSocket with the UR robot and sends messsages that contains .script commands to control the screwdriver tool. - """ - - def __init__( - self, - hostname: str = None, - port: int = 30020, - socket_timeout: float = 2.0, - ) -> None: - """ - Constructor for the RobotiqScrewdriver class. - - Args: - hostname (str): The hostname of the robot. - port (int): Port number to connect to the robot over the Interpreter socket - """ - self.connection = None - self.hostname = hostname - self.timeout = socket_timeout - - def connect(self) -> None: - """ - Creates an interpreter socket connection - """ - try: - self.connection = InterpreterSocket(hostname=self.hostname, timeout=self.timeout) - self.connection.connect() - except Exception as err: - print("Failed to connect to interpreter socket! " + err) - else: - print("Screwdriver connected") - - def disconnect(self) -> None: - """ - Closes the Interpreter socket connection - """ - self.connection.disconnect() - - def get_status(self) -> str: - """ - Retrieves the current activation status of the Screwdriver. - - Return (int): Status - - 3 = Activated - - 2 = Not used - - 1 = Activation in progress - - 0 = Else - """ - self.connection.execute_command("rq_get_screw_status()") - return self.connection.response - - def get_vacuum_pressure_kpa(self) -> str: - """ - Retrieves the current vacuum level in the vacuum sleeve. - - Return (int): Value = Current vacuum level in the vacuum sleeve in kPa. - Without screw vacuum must be equal or superior to -16 kPa - With screw vWacuum must be equal or inferior to -30 kPa - """ - self.connection.execute_command("rq_get_vacuum_pressure_kpa()") - return self.connection.response - - def is_blocked_at_start(self) -> str: - """ - The Screwdriver started fastening a screw on which torque has already been applied. - Use this script under the “ Unhandled errors” node to display the cause of the error. - - Return (bool): TRUE = The cause of the “ Unhandled errors” is “ Blocked at start” . - """ - self.connection.execute_command("rq_is_blocked_at_start_cause()") - return self.connection.response - - def is_communication_lost(self) -> str: - """ - The communication between the Screwdriver and the robot controller has been lost. - Use this script under the “ Unhandled errors” node to display the cause of the error. - - Return (bool): TRUE = The cause of the “ Unhandled errors” is “ Communication lost” . - - """ - self.connection.execute_command("rq_is_communication_lost_cause()") - return self.connection.response - - def is_contact_not_found(self) -> str: - """ - The Screwdriver did not find contact with the surface. - Use this script under the “ Unhandled errors” node to display the cause of the error. - - Return (bool): TRUE = The cause of the “ Unhandled errors” is “ Contact not found” . - """ - self.connection.execute_command("rq_is_contact_not_found_cause()") - return self.connection.response - - def is_feeder_screw_ready(self, inputNumber: int = 0) -> str: - """ - Validates if a screw is ready to be picked at the screw detection position of the Screw Feeder. - - Args: inputNumber (int): Integer associated with the “ Screw ready” digital input. - - Return (str): TRUE = A screw is detected by the screw detection sensor of the Screw Feeder. - - """ - self.connection.execute_command("rq_is_feeder_screw_ready({})".format(inputNumber)) - return self.connection.response - - def is_feeder_status_ok(self, inputNumber: int = 1) -> str: - """ - Validates if the Screw Feeder is on and ready. - - Args: inputNumber (int): Integer associated with the digital input of “ Feeder status” . - - Return (str): TRUE = The Screw Feeder is turned on and detects no errors. - - """ - self.connection.execute_command("rq_is_feeder_status_ok({})".format(inputNumber)) - return self.connection.response - - def is_restricted_before_angle(self) -> str: - """ - The torque value defined at the teaching step has been reached earlier than expected. - Use this script under the “ Unhandled errors” node to display the cause of the error. - - Return (str) TRUE = The cause of the “ Unhandled errors” is “ Restriction before angle” . - """ - self.connection.execute_command("rq_is_restricted_before_angle_cause()") - return self.connection.response - - def is_screw_detected(self) -> str: - """ - Validates the presence of a screw at the end of the screwdriver via vacuum action. The vacuum needs to be turned on for the system to be able to detect the screw. - Return (str): TRUE = A screw is detected at the end of the vacuum sleeve. - """ - self.connection.execute_command("rq_is_screw_detected()") - return self.connection.response - - def is_screwdriver_wrong_echo(self) -> str: - """ - There is a discrepancy between the command sent to the Screwdriver and the action performed by the Screwdriver. Cross threading may have caused this issue. - Use this script under the “ Unhandled errors” node to display the cause of the error. - - Return (str): TRUE = The cause of the “ Unhandled errors“ is “ Wrong echo” . - """ - self.connection.execute_command("rq_is_screwdriver_wrong_echo_cause()") - return self.connection.response - - def pick_screw_from_feeder( - self, - screw_pose: str = None, - status_input: int = 1, - ready_input: int = 0, - test_in_action: bool = False, - approach_dist: int = 1, - rpm: int = 100, - pick_speed: int = 1, - timeout: int = 10, - retract_speed: int = 1, - direction: bool = False, - slave_id: int = 9, - ) -> None: - """ - Picks a screw from the Robotiq Screw Feeder. - Args: - screw_pose (str): Position where the robot will be picking the screw. Defined in Installation under Features. - - status_input (int): Digital input used for “ Feeder status” input. - - ready_input (int): Digital input used for “ Screw ready” input. - - test_in_action (bool): TRUE = The robot stops after picking a screw. - FALSE = The robot moves on to the next line in the program after picking a screw. - - approach_dist (int): (unit: mm) Distance between the approach point and the screw_pose point - - rpm (int): (unit: RPM) Desired rotation speed of the screwdriver. Must be between 1 and 500 RPM - - pick_speed (int): (unit: mm/s) Movement speed between the approach point and the screw_pose point. - - timeout (int): (unit: sec) Time after which the robot will stop trying to pick a screw if the previous pick was unsuccessful. - - retract_speed (int): (unit: mm/s) Movement speed between the screw_pose point and the retract point. - - direction (bool): FALSE = RQ_DIRECTION_CCW (default) for a counter-clockwise rotation of the Screwdriver. - TRUE = RQ_DIRECTION_CW for a clockwise rotation of the Screwdriver. - - slave_id (int): Only slave ID 9 is supported by the Screwdriver. - """ - self.connection.execute_command( - "rq_pick_screw({},{},{},{},{},{},{},{},{},{},{})".format( - screw_pose, - status_input, - ready_input, - test_in_action, - approach_dist, - rpm, - pick_speed, - timeout, - retract_speed, - direction, - slave_id, - ) - ) - - def activate_screwdriver(self) -> None: - """ - Activates the Screwdriver once it is powered on. - To ensure the consistent activation of the Screwdriver at the start of the program, it is recommended to insert a rq_screw_activate() script function in the BeforeStart sequence of the program. - """ - self.connection.execute_command("rq_screw_activate()") - - def stop_screwdriver(self) -> None: - """ - Stops the rotation of the screwdriver. - """ - self.connection.execute_command("rq_screw_stop()") - - def activate_vacuum(self) -> None: - """ - Starts the vacuum action on the Screwdriver. This is required for the system to be able to detect a screw. - """ - self.connection.execute_command("rq_screw_vacuum_on()") - - def deactivate_vacuum(self) -> None: - """ - Stops the vacuum action on the Screwdriver. - """ - self.connection.execute_command("rq_screw_vacuum_off()") - - def drive_clockwise( - self, - mode: int = 1, - torque: int = 1, - angle: int = 360, - rpm: int = 100, - ) -> None: - """ - Turns the bit of the Screwdriver in clock wise direction around by a number of degrees or until a specific torque value is reached. - Accuracy of the final torque value may vary from standard tolerances. - - Args: - mode (int): 1 = RQ_ REGULATION_MODE_ANGLE to turn until a specific number of turns is achieved. - 3 = RQ_REGULATION_MODE_SPEED to turn until a specific torque value is reached. - - torque (int): (unit:Nm) The torque value at which the Screwdriver will stop turning. - Must be between 1 and 4 Nm. Only read when RQ_REGULATION_MODE_SPEED is used. - - angle (int): (unit: degrees (°)) The number of degrees by which to turn before the Screwdriver stops. - Only read when RQ_REGULATION_MODE_ANGLE is used. - Note: Use “ 0” for rotation_angle in case it is not required. - - rpm (int): (unit: RPM) Desired rotation speed of the screwdriver. Must be between 1 and 500 RPM - - """ - direction = True # TRUE = RQ_DIRECTION_CW for a clockwise rotation of the Screwdriver. - slave_id = 9 # Only slave ID 9 is supported by the Screwdriver. - - self.connection.execute_command( - "rq_screw_turn({},{},{},{},{},{})".format( - mode, - torque, - angle, - rpm, - direction, - slave_id, - ) - ) - - def drive_counter_clockwise( - self, - mode: int = 1, - torque: int = 1, - angle: int = 360, - rpm: int = 100, - ) -> None: - """ - Turns the bit of the Screwdriver in counter clock wise direction around by a number of degrees or until a specific torque value is reached. - Accuracy of the final torque value may vary from standard tolerances. - - Args: - mode (int): 1 = RQ_ REGULATION_MODE_ANGLE to turn until a specific number of turns is achieved. - 3 = RQ_REGULATION_MODE_SPEED to turn until a specific torque value is reached. - - torque (int): (unit:Nm) The torque value at which the Screwdriver will stop turning. - Must be between 1 and 4 Nm. Only read when RQ_REGULATION_MODE_SPEED is used. - - angle (int): (unit: degrees (°)) The number of degrees by which to turn before the Screwdriver stops. - Only read when RQ_REGULATION_MODE_ANGLE is used. - Note: Use “ 0” for rotation_angle in case it is not required. - - rpm (int): (unit: RPM) Desired rotation speed of the screwdriver. Must be between 1 and 500 RPM - - """ - direction = False # FALSE= RQ_DIRECTION_CCW for a counter-clockwise rotation of the Screwdriver. - slave_id = 9 # Only slave ID 9 is supported by the Screwdriver. - - self.connection.execute_command( - "rq_screw_turn({},{},{},{},{},{})".format( - mode, - torque, - angle, - rpm, - direction, - slave_id, - ) - ) - - def auto_unscrew(self, rpm: int = 100) -> None: - """ - With the screwdriving bit in or just above the screw drive, performs a full unscrew action until the screw threads are disengaged - from the hole. - - Args: - rpm: (unit: RPM) - Desired rotation speed of the screwdriver. Must be between 1 and 500 RPM - """ - direction = False # FALSE = RQ_DIRECTION_CCW (default) for a counter-clockwise rotation of the Screwdriver - self.connection.execute_command("rq_auto_unscrew({}, {})".format(direction, rpm)) - - def auto_screw(self, rpm: int = 100) -> None: - """ - With the screwdriving bit in or just above the screw drive, performs a full screw action until the screw threads are disengaged - from the hole. - - Args: - rpm (int): (unit: RPM) Desired rotation speed of the screwdriver. Must be between 1 and 500 RPM - """ - direction = True # TRUE = RQ_DIRECTION_CW for a clockwise rotation of the Screwdriver - self.connection.execute_command("rq_auto_unscrew({}, {})".format(direction, rpm)) - - -if __name__ == "__main__": - tool = RobotiqScrewdriver("164.54.116.129") # replace with your device's IP - tool.connect() - # tool.activate_vacuum() - # sleep(10) - tool.drive_clockwise(rpm=250, angle=3600) - # sleep(8) - # tool.deactivate_vacuum() - # tool.auto_unscrew() - # tool.drive_clockwise() # sleep(10) - # tool.drive_counter_clockwise() diff --git a/src/ur_interface/ur_tools/screwdriver_controller.py b/src/ur_interface/ur_tools/screwdriver_controller.py deleted file mode 100644 index 2ceab58..0000000 --- a/src/ur_interface/ur_tools/screwdriver_controller.py +++ /dev/null @@ -1,197 +0,0 @@ -"""Screwdriver Controller Class""" - -import os -from copy import deepcopy -from time import sleep - -from .robotiq_screwdriver_driver import RobotiqScrewdriver - - -class ScrewdriverController: - """This class is designed to control Robotiq Screwdriver tools and UR driver to perform screwdriwing tasks""" - - def __init__( - self, - hostname: str = None, - ur=None, - ur_dashboard=None, - ): - """Constracter of the ScrewdriverController class which initializes a connection with the Robotiq Screwdriver tool and UR robot interfaces""" - # TODO: Make sure interpreter urp program exsists on the polyscope then start the program using the UR Dashboard. - # TODO: Import screwdriver driver and handle all the motions as well as screwdriving jobs here - - self.hostname = hostname - self.ur = None - self.air_switch_digital_output = 0 - - current_dir = os.getcwd() - index = current_dir.find("ur_module") - parent_dir = current_dir[: index + 10] - self.interpreter_urp = parent_dir + "/ur_driver/scripts/urp_programs/interpreter_mode.urp" - - if not ur: - raise Exception("Failed to receive UR connection!") - else: - self.ur_dashboard = ur_dashboard - self.ur = ur - self.ur.set_payload(3) - - try: - self.screwdriver = RobotiqScrewdriver(hostname=self.hostname, socket_timeout=5) - self.screwdriver.connect() - except Exception as err: - print(err) - - self.load_interpreter_socket_program() - - def load_interpreter_socket_program(self): - """ - Makes sure that the interpreter socket is enabled on the robot PolyScope so that screwdriver commands can be sent over this socket. - """ - iterpreter_program = "/programs/interpreter_mode.urp" - response = self.ur_dashboard.load_program(iterpreter_program) - if "File not found" in response: - self.ur_dashboard.transfer_program( - local_path=self.interpreter_urp, - ur_path=iterpreter_program, - ) - response = self.ur_dashboard.load_program(iterpreter_program) - self.ur_dashboard.run_program() - sleep(2) - - def pick_screw( - self, - screw_loc: list = None, - approach_axis: str = None, - approach_distance: float = None, - ): - """Picks up a new screw.""" - if not screw_loc: - raise Exception("Please provide the source loaction") - - if not approach_distance: - approach_distance = 0.02 - - axis = None - - if not approach_axis or approach_axis.lower() == "z": - axis = 2 - elif approach_axis.lower() == "y": - axis = 1 - elif approach_axis.lower() == "-y": - axis = 1 - approach_distance = -approach_distance - elif approach_axis.lower() == "x": - axis = 0 - elif approach_axis.lower() == "-x": - axis = 0 - approach_distance = -approach_distance - - screw_above = deepcopy(screw_loc) - screw_above[axis] += approach_distance - screw_start_loc = deepcopy(screw_loc) - screw_start_loc[axis] += 0.01 - - print("Picking up the screw...") - - self.ur.movel(screw_above, 1, 1) - self.ur.movel(screw_start_loc, 0.5, 0.5) - self.ur.set_digital_out(self.air_switch_digital_output, True) - self.ur_dashboard.run_program() # Restart interpreter program - sleep(2) - self.screwdriver.activate_vacuum() - self.screwdriver.auto_screw() - sleep(4) - self.ur.movel(screw_above, 1, 0.5) - sleep(2) - - def place_screw( - self, - target: list = None, - approach_axis: str = None, - approach_distance: float = None, - ): - """ - Attempts to screws down the screw into the target location - """ - if not target: - raise Exception("Please provide the target loaction") - - if not approach_distance: - approach_distance = 0.02 - - axis = None - - if not approach_axis or approach_axis.lower() == "z": - axis = 2 - elif approach_axis.lower() == "y": - axis = 1 - elif approach_axis.lower() == "-y": - axis = 1 - approach_distance = -approach_distance - elif approach_axis.lower() == "x": - axis = 0 - elif approach_axis.lower() == "-x": - axis = 0 - approach_distance = -approach_distance - - target_above = deepcopy(target) - - target_above[axis] += approach_distance - - print("Placing the screw to the target...") - sleep(1) - self.ur.movel(target_above, 1, 1) - self.ur.set_digital_out(self.air_switch_digital_output, True) - - self.ur.movel(target, 1, 1) - sleep(1) - self.ur_dashboard.run_program() # Restart interpreter program - sleep(2) - self.screwdriver.activate_vacuum() - self.screwdriver.auto_screw(250) - sleep(2) - self.screwdriver.drive_clockwise(angle=200, rpm=100) - sleep(2) - self.screwdriver.deactivate_vacuum() - self.ur.set_digital_out(self.air_switch_digital_output, False) - sleep(1) - self.ur.movel(target_above, 0.5, 0.5) - sleep(2) - # self.ur_dashboard.run_program() #Restart interpreter program - # sleep(2) - print("Screw successfully placed") - - # if self.screwdriver.is_screw_detected() == "False": - # print("Screw successfully placed") - # # self.screwdriver.deactivate_vacuum() - # else: - # print("Failed to place the screw") - - def transfer( - self, - source: list = None, - target: list = None, - source_approach_axis: str = None, - target_approach_axis: str = None, - source_approach_dist: float = None, - target_approach_dist: float = None, - ) -> None: - """Handles a screw transfer""" - - self.pick_screw( - screw_loc=source, - approach_axis=source_approach_axis, - approach_distance=source_approach_dist, - ) - print("Pick screw completed") - self.place_screw( - target=target, - approach_axis=target_approach_axis, - approach_distance=target_approach_dist, - ) - print("Place screw completed") - - -if __name__ == "__main__": - screwdrive = ScrewdriverController(hostname="164.54.116.129") diff --git a/src/ur_interface/ur_tools/tricontinent_pipette_controller.py b/src/ur_interface/ur_tools/tricontinent_pipette_controller.py deleted file mode 100644 index 2b7450c..0000000 --- a/src/ur_interface/ur_tools/tricontinent_pipette_controller.py +++ /dev/null @@ -1,373 +0,0 @@ -"""Tricontinent Pipette Controller Class""" - -from copy import deepcopy -from time import sleep - -from .pipette_driver import PipetteDriver - - -class TricontinentPipetteController: - """This class aims to control tricontinent pipetes by utilizing a socket connection with the pipette over PipetteDriver class while controlling the UR robot simultaneously""" - - def __init__( - self, - hostname=None, - ur=None, - pipette_ip: str = None, - resource_client=None, - pipette_resource_id: str = None, - ) -> None: - """ - Initializes the PipetteController class. - - Parameters: - - pipette_pv (str): The EPICS process variable (PV) for the pipette. - - ur_connection: The connection object for the Universal Robot (UR) robot. - """ - - self.hostname = hostname - self.resource_client = resource_client - self.pipette_resource_id = pipette_resource_id - - if not ur or not pipette_ip: - raise Exception("UR connection is not established") - else: - self.ur = ur - self.IP = pipette_ip - - self.acceleration = 0.5 - self.speed_fast = 0.750 - self.speed_slow = 0.1 - self.ref_frame = [0, 0, 0, 0, 0, 0] - - self.pipette_drop_tip_value = -8 - self.pipette_aspirate_value = 2.0 - self.pipette_dispense_value = -2.0 - self.droplet_value = 0.3 - - def connect_pipette(self, speed: int = 150): - """ - Connects to the pipette by first setting the correct tool communication parameters - """ - for i in range(5): - try: - # Establishing a connection with the pipette on RS485 comminication - self.pipette = PipetteDriver() - comm_setting = self.pipette._comm_setting - self.ur.set_tool_communication( - baud_rate=comm_setting["baud_rate"], - parity=comm_setting["parity"], - stop_bits=comm_setting["stop_bits"], - rx_idle_chars=comm_setting["rx_idle_chars"], - tx_idle_chars=comm_setting["tx_idle_chars"], - ) - sleep(2) - self.pipette.connect(hostname=self.IP) - - error = True - timout = 0 - while error: - try: - sleep(1) - timout += 1 - self.pipette.set_speed(start=speed, top=speed, stop=speed) - except Exception as e: - print("Error in setting speed:", e) - else: - error = False - if timout > 30: - raise TimeoutError("Timeout while setting pipette speed") - - except Exception as err: - print("Pipette connection error: ", err) - - else: - print("Pipette is connected after {} tries".format(i + 1)) - break - - def initialize_pipette(self): - """ - Initializes the pipette by setting the correct tool communication parameters - and connecting to the pipette. - """ - try: - self.pipette.initialize() - # sleep(5) - - except Exception as err: - print("Pipette initialization error: ", err) - - else: - print("Pipette is initialized") - - def disconnect_pipette(self): - """ - Disconnect pipette - """ - - try: - self.pipette.disconnect() - # self.ur.set_tool_communication(enabled=False) - - except Exception as err: - print("Pipette disconnection error: ", err) - - else: - print("Pipette is disconnected") - - def pick_tip(self, tip_loc, x=0, y=0): - """ - Description: Picks up a new tip from the first location on the pipette bin. - """ - - tip_approach = deepcopy(tip_loc) - tip_approach[2] += 0.02 - tip_above = deepcopy(tip_loc) - tip_above[2] += 0.1 - - print("Picking up the first pipette tip...") - - self.ur.movel(tip_above, self.acceleration, self.speed_fast) - # sleep(2) - self.ur.movel(tip_approach, self.acceleration, self.speed_fast) - # sleep(2) - self.ur.movel(tip_loc, self.acceleration, self.speed_slow) - # sleep(3) - self.ur.movel(tip_approach, self.acceleration, self.speed_slow) - # sleep(2) - self.ur.movel(tip_above, self.acceleration, self.speed_fast) - # sleep(2) - print("Pipette tip successfully picked up") - - def transfer_sample( - self, - home: list = None, - safe_waypoint: list = None, - sample_aspirate: list = None, - sample_dispense: list = None, - volume: int = 10, - speed: float = 150, - ): - """ - Description: - - Makes a new sample on the 96 well plate. - - Mixes to liquits in a single well and uses a new pipette tip for each liquid. - - In order to mix the liquids together, pipette performs aspirate and dispense operation multiple times in the well that contains both the liquids. - """ - print("Making a sample using two liquids...") - - sample_aspirate_above = deepcopy(sample_aspirate) - sample_aspirate_above[2] += 0.05 - - self.ur.movel( - sample_aspirate_above, - self.acceleration, - self.speed_fast, - ) - self.ur.movel(sample_aspirate, self.acceleration, self.speed_slow) - - self.pipette.set_speed(start=speed, top=speed, stop=speed) - self.pipette.aspirate(vol=volume) - - if self.resource_client: - self.resource_client.increase_quantity( - resource=self.pipette_resource_id, - amount=volume, - ) - - self.ur.movel(sample_aspirate_above, self.acceleration, self.speed_slow) - if safe_waypoint: - self.ur.movel(safe_waypoint, self.acceleration, self.speed_fast) - else: - self.ur.movej(home, self.acceleration, self.speed_fast) - - sample_dispense_above = deepcopy(sample_dispense) - sample_dispense_above[2] += 0.02 - self.ur.movel( - sample_dispense_above, - self.acceleration, - self.speed_fast, - ) - self.ur.movel(sample_dispense, self.acceleration, self.speed_slow) - - self.pipette.dispense(vol=volume) - sleep(5) - if self.resource_client: - self.resource_client.decrease_quantity( - resource=self.pipette_resource_id, - amount=volume, - ) - self.ur.movel( - sample_dispense_above, - self.acceleration, - self.speed_slow, - ) - if safe_waypoint: - self.ur.movel(safe_waypoint, self.acceleration, self.speed_fast) - else: - self.ur.movej(home, self.acceleration, self.speed_fast) - - def pick_and_move( - self, - safe_waypoint: list = None, - sample_loc: list = None, - target: list = None, - volume: int = 10, - ): - """ - Description: - - Picks up a sample from the sample location and moves it to the target location. - - The sample is picked up using the pipette and then moved to the target location but not dispensed. - """ - - sample_loc_above = deepcopy(sample_loc) - sample_loc_above[2] += 0.05 - - self.ur.movel( - sample_loc_above, - self.acceleration, - self.speed_fast, - ) - self.ur.movel(sample_loc, self.acceleration, self.speed_slow) - - self.pipette.aspirate(vol=volume) - # sleep(5) - if self.resource_client: - self.resource_client.increase_quantity( - resource=self.pipette_resource_id, - amount=volume, - ) - - self.ur.movel(sample_loc_above, self.acceleration, self.speed_fast) - if safe_waypoint: - self.ur.movel(safe_waypoint, self.acceleration, self.speed_fast) - target_above = deepcopy(target) - target_above2 = deepcopy(target) - target_above[2] += 0.05 - # target_above2[2] += 0.01 - target_above2[2] += 0.030 - self.ur.movel( - target_above, - self.acceleration, - self.speed_fast, - wait=True, - ) - self.ur.movel( - target_above2, - self.acceleration, - self.speed_slow, - ) - - def dispense_and_retrieve( - self, - safe_waypoint: list = None, - target: list = None, - volume: int = 10, - ): - """ - Description: - - Dispenses the sample at the target location and retrieves the pipette. - - The sample is dispensed using the pipette and then the robot is moved back to the home position. - """ - self.ur.movel(target, self.acceleration, self.speed_slow) - - self.pipette.dispense(vol=volume) - # sleep(3 * (150 / speed)) # Adjust sleep time based on speed - - if self.resource_client: - self.resource_client.decrease_quantity( - resource=self.pipette_resource_id, - amount=volume, - ) - - target_above = deepcopy(target) - target_above[2] += 0.05 - self.ur.movel( - target_above, - self.acceleration, - self.speed_fast, - wait=True, - ) - if safe_waypoint: - self.ur.movel(safe_waypoint, self.acceleration, self.speed_fast) - - def create_droplet(self, droplet_loc): - """ - Description: - - Drives pipette to create a droplet. - - Number of motor steps to create a droplet is stored in "self.droplet_value". - - Pipette is controlled by RS485 commication. - """ - print("Creating a droplet...") - droplet_front = deepcopy(droplet_loc) - droplet_front[1] += 0.1 - self.ur.movel(droplet_front, self.acceleration, self.speed_fast) - self.ur.movel(droplet_loc, self.acceleration, self.speed_fast) - sleep(1) - self.pipette.dispense(vol=3) - sleep(5) - self.pipette.aspirate(vol=3) - sleep(1) - self.ur.movel(droplet_front, self.acceleration, self.speed_slow) - - def empty_tip(self, sample_loc): - """ - Description: - - Dispense all the sample inside the pipette. - - """ - print("Emtying the tip...") - - # MOVE TO THE FIRT SAMPLE LOCATION - sample_above = deepcopy(sample_loc) - sample_above[2] += 0.1 - - self.ur.movel(sample_above, self.acceleration, self.speed_fast) - self.ur.movel(sample_loc, self.acceleration, self.speed_fast) - self.pipette.dispense(vol=25) - sleep(2) - self.ur.movel(sample_above, self.acceleration, self.speed_fast) - - def eject_tip( - self, - eject_tip_loc: list = None, - approach_axis: str = "x", - approach_distance: float = -0.005, - ): - """ - Description: Drops the pipette tip into trash bin - """ - if approach_axis.lower() == "y": - axis = 0 - else: - axis = 1 - - trash_above = deepcopy(eject_tip_loc) - trash_front = deepcopy(eject_tip_loc) - trash_above[2] += 0.1 - trash_front[axis] += approach_distance - trash_front_above = deepcopy(trash_front) - trash_front_above[2] += 0.1 - - print("Droping tip to the trash bin...") - # Move to the trash bin location - self.ur.movel(trash_front_above, self.acceleration, self.speed_fast) - self.ur.movel(trash_front, self.acceleration, self.speed_fast) - self.ur.movel(eject_tip_loc, self.acceleration, self.speed_fast) - self.ur.movel(trash_above, self.acceleration, self.speed_fast) - - -if __name__ == "__main__": - from urx import Robot - - # r = Robot("164.54.116.129") - ip = "192.168.1.102" - r = Robot(ip) - a = TricontinentPipetteController(ur=r, pipette_ip=ip) - a.connect_pipette() - sleep(5) - a.pipette.initialize() - a.pipette.aspirate(vol=5) - sleep(5) - a.disconnect_pipette() - r.close() diff --git a/src/ur_interface/ur_tools/urp_generator.py b/src/ur_interface/ur_tools/urp_generator.py deleted file mode 100644 index 84b9210..0000000 --- a/src/ur_interface/ur_tools/urp_generator.py +++ /dev/null @@ -1,154 +0,0 @@ -"""URP Generator Class""" - - -class URPGenerator: - """This object is designed to genereate URP scripts on local to be later trasnfer on to the Polyscope""" - - def __init__(self, filename: str = None): - """Contractor for the URPGenerator - - Args: - filename (str): Filename to be used after the URP program is generated - """ - self.filename = filename - self.program = "" - - def add_line(self, line: str = None): - """Adds a new line - - Args: - line (str): A string to be added as a new line - """ - self.program += line + "\n" - - def add_activate_tool(self, tool_name: str = None): - """Adds the necessary lines to activate a tool into the URP program - - Args: - tool_name (str): Name of the tool - """ - self.add_line("def activate_tool():") - self.add_line(" textmsg('Activating Robotiq screwdriver tool')") - self.add_line("") - self.add_line(" # Set tool voltage") - self.add_line(" set_tool_voltage(24)") - self.add_line("") - self.add_line(" # Set tool digital output to activate screwdriver") - self.add_line(" set_tool_digital_out(0, True)") - self.add_line("") - self.add_line(" # Wait for activation") - self.add_line(" sleep(2)") # Adjust the sleep duration as needed - self.add_line("") - self.add_line(" textmsg('Robotiq screwdriver tool activated')") - self.add_line("") - self.add_line("end") - - def add_pick_and_place( - self, - pick_location: str = None, - place_location: str = None, - ): - """Adds the necessary lines for pick and place jobs into the URP program - - Args: - pick_location (str): Pick location - place_location (str): Place location - """ - self.add_line("def pick_and_place():") - self.add_line(" textmsg('Starting pick and place operation')") - self.add_line("") - self.add_line(" # Move to pick location") - self.add_line(f" movel(p{pick_location}, a=0.5, v=0.5)") - self.add_line(" textmsg('Reached pick location')") - self.add_line("") - self.add_line(" # Perform pick action") - self.add_line(" # Add your pick action here") - self.add_line("") - self.add_line(" # Move to place location") - self.add_line(f" movel(p{place_location}, a=0.5, v=0.5)") - self.add_line(" textmsg('Reached place location')") - self.add_line("") - self.add_line(" # Perform place action") - self.add_line(" # Add your place action here") - self.add_line("") - self.add_line(" textmsg('Pick and place operation completed')") - self.add_line("") - self.add_line("end") - - def add_drive_screw( - self, - torque: float = None, - rotation_speed: float = None, - ): - """Adds the necessary lines for driving the screwdriver into the URP program - - Args: - torque (float): NM - rotation_speed (float): RPM - """ - self.add_line("def drive_screw(torque, rotation_speed):") - self.add_line(" textmsg('Driving screw')") - self.add_line("") - self.add_line(" # Set screwdriving torque") - self.add_line(" set_tool_digital_out(1, True)") - self.add_line(" set_analog_out(0, torque)") - self.add_line("") - self.add_line(" # Rotate screwdriver at specified speed") - self.add_line(" set_analog_out(1, rotation_speed)") - self.add_line("") - self.add_line(" # Wait for screwdriving") - self.add_line(" sleep(2)") # Adjust the sleep duration as needed - self.add_line("") - self.add_line(" # Stop rotating screwdriver") - self.add_line(" set_analog_out(1, 0)") - self.add_line("") - self.add_line(" textmsg('Screwdriving completed')") - self.add_line("") - self.add_line("end") - - def add_deactivate_tool(self): - """Adds the necessary lines to deactivate the tool into the URP program""" - self.add_line("def deactivate_tool():") - self.add_line(" textmsg('Deactivating Robotiq screwdriver tool')") - self.add_line("") - self.add_line(" # Set tool digital outputs to deactivate screwdriver") - self.add_line(" set_tool_digital_out(0, False)") - self.add_line(" set_tool_digital_out(1, False)") - self.add_line("") - self.add_line(" # Set analog outputs to zero") - self.add_line(" set_analog_out(0, 0)") - self.add_line(" set_analog_out(1, 0)") - self.add_line("") - self.add_line(" textmsg('Robotiq screwdriver tool deactivated')") - self.add_line("") - self.add_line("end") - - def generate_program(self): - """Gerates the program and saves it to local path""" - with open(self.filename, "w") as file: - file.write(self.program) - - print(f".urp program '{self.filename}' generated successfully.") - - -# Usage example -pick_location = [ - 0.1, - 0.2, - 0.3, - 0, - 3.14159, - 0, -] # Example pick location (x, y, z, rx, ry, rz) -place_location = [ - 0.4, - 0.5, - 0.6, - 0, - 3.14159, - 0, -] # Example place location (x, y, z, rx, ry, rz) - -# generator = URPGenerator("pick_and_place.urp") -# generator.add_pick_and_place(pick_location, place_location) -# generator.generate_program() diff --git a/src/ur_interface/ur_tools/wm_tool_changer_controller.py b/src/ur_interface/ur_tools/wm_tool_changer_controller.py deleted file mode 100644 index f0c3d08..0000000 --- a/src/ur_interface/ur_tools/wm_tool_changer_controller.py +++ /dev/null @@ -1,205 +0,0 @@ -"""WingMan Tool Changer Controller Class""" - -from copy import deepcopy -from time import sleep -from typing import Union - -from madsci.common.types.location_types import LocationArgument - - -class WMToolChangerController: - """Initilizes the WMToolChangerController to pick and place tools with the Wingman tool changers""" - - def __init__( - self, - tool_location: Union[LocationArgument, list] = None, - docking_axis: str = "y", - ur=None, - tool: str = None, - resource_client=None, - tool_resource_id: str = None, - ): - """Constractor class - - Args: - tool_location (list): Tool location - docking axis (str): Docking axis based on the robot location - ur (urx.Robot): UR robot connection - tool (str): Tool name - """ - if not ur: - raise AttributeError("Ur connection is not provided") - - self.ur = ur - self.resource_client = resource_client - self.tool_resource_id = tool_resource_id - self.tool = tool - self.current_tool = None - self.axis = docking_axis - - if not tool_location: - raise Exception("Please provide a target location for the tool") - else: - self.location = tool_location - - self._set_target_locations() - self.robot_fast_acceleration = 1.0 - self.robot_fast_velocity = 1.0 - self.robot_slow_acceleration = 0.2 - self.robot_slow_velocity = 0.2 - - self.gripper_com = { - "baud_rate": 115200, - "parity": 0, - "stop_bits": 1, - "rx_idle_chars": 1.5, - "tx_idle_chars": 3.5, - } - - self.pipette_com = { - "baud_rate": 9600, - "parity": 0, - "stop_bits": 1, - "rx_idle_chars": 1.5, - "tx_idle_chars": 3.5, - } - - def _set_target_locations(self): - """Sets the target locations for the tool changer""" - if isinstance(self.location, LocationArgument): - self.location_joint_values = self.location.location - elif isinstance(self.location, list): - self.location_joint_values = self.location - - self.tool_above = deepcopy(self.location_joint_values) - self.tool_above[2] += 0.05 - self.tool_front = None - self._get_tool_front() - self.tool_front_above = deepcopy(self.tool_front) - self.tool_front_above[2] += 0.25 - - def _set_tool_params(self, tool: str = None): - """Sets Tool Parameters - - Args: - tool (str): Tool name - """ - if tool: - if tool.lower() == "gripper": - self._set_tool_communication(tool=self.gripper_com) - sleep(4) - if self.discover_tool(): - pass - else: - pass - - elif tool.lower() == "pipette": - self._set_tool_communication(tool=self.pipette_com) - sleep(4) - if self.discover_tool(): - pass - else: - pass - - def _set_tool_communication(self, tool: dict = None): - """Sets tool communication using the tool specific parameters - - Args: - tool (dict): tool specific parameters to set the correct Tool I/O - """ - if tool: - self.ur.set_tool_communication( - baud_rate=tool["baud_rate"], - parity=tool["parity"], - stop_bits=tool["stop_bits"], - rx_idle_chars=tool["rx_idle_chars"], - tx_idle_chars=tool["tx_idle_chars"], - ) - - def _get_tool_front(self): - """Gets the tool front location""" - self.tool_front = deepcopy(self.location_joint_values) - if self.axis == "x": - self.tool_front[0] += 0.1 - elif self.axis == "-x": - self.tool_front[0] -= 0.1 - elif self.axis == "y": - self.tool_front[1] += 0.1 - elif self.axis == "-y": - self.tool_front[1] -= 0.1 - - # TODO: Use the tool location to find which quadrant will the robot be using (x,y) and then find the 6th joint rotation to figure out which direction the robot is looking at to extract the horizatal movement axis - - def get_tool_status(self): - """Gets the tool changer current status. - - Returns (str): Status string - """ - status = "STATUS" - return status - - def pick_tool(self): - """Picks a new tool using the tool location.""" - try: - print("Picking up the tool...") - self.ur.movel( - self.tool_above, - self.robot_fast_acceleration, - self.robot_fast_velocity, - ) - self.ur.movel( - self.location_joint_values, - self.robot_slow_acceleration, - self.robot_slow_velocity, - ) - self.ur.movel( - self.tool_front, - self.robot_slow_acceleration, - self.robot_slow_velocity, - ) - self.ur.movel( - self.tool_front_above, - self.robot_fast_acceleration, - self.robot_fast_velocity, - ) - - except Exception as err: - print( - "Error accured while picking up the tool changer: ", - err, - ) - - def place_tool(self): - """Places the currently attached tool back to the initial tool location""" - try: - print("Placing the tool ...") - self.ur.movel( - self.tool_front_above, - self.robot_fast_acceleration, - self.robot_fast_velocity, - ) - self.ur.movel( - self.tool_front, - self.robot_fast_acceleration, - self.robot_fast_velocity, - ) - self.ur.movel( - self.location_joint_values, - self.robot_slow_acceleration, - self.robot_slow_velocity, - ) - self.ur.movel( - self.tool_above, - self.robot_slow_acceleration, - self.robot_slow_velocity, - ) - except Exception as err: - print( - "Error accured while placing the tool: ", - err, - ) - - def discover_tool(self): - """Discover if a tool is currently attached and which tool it is.""" - ### Check resources DB for the tool - pass diff --git a/src/ur_rest_node.py b/src/ur_rest_node.py index 560762b..7ab02a6 100644 --- a/src/ur_rest_node.py +++ b/src/ur_rest_node.py @@ -1,21 +1,18 @@ """REST-based node for UR robots""" -import traceback from typing import Optional, Union -from madsci.common.types.action_types import ActionFailed, ActionSucceeded +from madsci.client.resource_client import ResourceClient from madsci.common.types.admin_command_types import AdminCommandResponse from madsci.common.types.location_types import LocationArgument from madsci.common.types.node_types import RestNodeConfig from madsci.common.types.resource_types import Pool, Slot from madsci.node_module.helpers import action from madsci.node_module.rest_node_module import RestNode +from pydantic import AnyUrl from typing_extensions import Annotated -from ur_interface.ur import UR -from ur_interface.ur_error_types import GripperError, URMovementError -from ur_interface.ur_kinematics import get_pose_from_joint_angles -from ur_interface.ur_tools.gripper_controller import FingerGripperController +from ur_interface.integrated_controller import IntegratedController, UREndEffector class URNodeConfig(RestNodeConfig): @@ -25,112 +22,109 @@ class URNodeConfig(RestNodeConfig): tcp_pose: list = [0, 0, 0, 0, 0, 0] base_reference_frame: Optional[list] = None ur_model: str = "UR5e" - use_resources: bool = True + end_effector: Optional[UREndEffector] = None + resource_manager_url: Optional[AnyUrl] = None class URNode(RestNode): """A Rest Node object to control UR robots""" - ur_interface: UR = None + integrated_controller: IntegratedController = None config: URNodeConfig = URNodeConfig() config_model = URNodeConfig def startup_handler(self) -> None: """Called to (re)initialize the node. Should be used to open connections to devices or initialize any other resources.""" - try: - # Create templates - if self.config.use_resources: - self._create_ur_templates() - - self.logger.log("Node initializing...") - self.ur_interface = UR( - hostname=self.config.ur_ip, - resource_client=self.resource_client if self.config.use_resources else None, - tcp_pose=self.config.tcp_pose, - base_reference_frame=self.config.base_reference_frame, - logger=self.logger, - ) - self.tool_resource = None - self.current_location = None - except Exception as err: - self.logger.log_error(f"Error starting the UR Node: {err}\n{traceback.format_exc()}") - self.startup_has_run = False - else: - self.startup_has_run = True - self.logger.log("UR node initialized!") - - def _create_ur_templates(self) -> None: + self.resource_client = None + self.end_effector_resource_id = None + if self.config.resource_manager_url is not None: + self.resource_client = ResourceClient(self.config.resource_manager_url) + self._create_ur_resources() + self.logger.log("Node initializing...") + self.integrated_controller = IntegratedController( + hostname=self.config.ur_ip, + resource_client=self.resource_client if self.config.resource_manager_url else None, + end_effector_resource_id=self.end_effector_resource_id, + tcp_pose=self.config.tcp_pose, + base_reference_frame=self.config.base_reference_frame, + end_effector=self.config.end_effector, + logger=self.logger, + ) + self.tool_resource = None + self.current_location = None + self.startup_has_run = True + self.logger.log("UR node initialized!") + + def _create_ur_resources(self) -> None: """Create all UR-specific resource templates.""" - - # 1. Gripper slot template - gripper_slot = Slot( - resource_name="robotiq_finger_gripper", - resource_class="URGripper", - capacity=1, - attributes={ - "gripper_type": "robotiq_finger", - "max_grip_force": 235.0, - "min_grip_position": 0, - "max_grip_position": 255, - "description": "UR Robotiq finger gripper slot", - }, - ) - - self.resource_client.init_template( - resource=gripper_slot, - template_name="robotiq_finger_gripper_slot", - description="Template for UR Robotiq finger gripper slot. Used to track what the gripper is holding.", - required_overrides=["resource_name"], - tags=["ur", "gripper", "slot", "robotiq"], - created_by=self.node_definition.node_id, - version="1.0.0", - ) - - # Initialize gripper resource from template - self.gripper_resource = self.resource_client.create_resource_from_template( - template_name="robotiq_finger_gripper_slot", - resource_name=f"ur_gripper_{self.node_definition.node_name}", - add_to_database=True, - ) - - # 2. Pipette pool template - pipette_pool = Pool( - resource_name="tricontinent_pipette", - resource_class="URPipette", - capacity=1000.0, - attributes={ - "pipette_type": "tricontinent", - "min_volume": 1.0, - "max_volume": 1000.0, - "default_speed": 150, - "description": "Tricontinent pipette pool for tracking tips and aspirated liquid", - }, - ) - - self.resource_client.init_template( - resource=pipette_pool, - template_name="tricontinent_pipette_pool", - description="Template for Tricontinent pipette pool. Tracks pipette tips and aspirated liquids.", - required_overrides=["resource_name"], - tags=["ur", "pipette", "pool", "liquid-handling"], - created_by=self.node_definition.node_id, - version="1.0.0", - ) - # Initialize pipette resource from template - self.pipette_resource = self.resource_client.create_resource_from_template( - template_name="tricontinent_pipette_pool", - resource_name=f"ur_pipette_{self.node_definition.node_name}", - add_to_database=True, - ) + if self.config.end_effector == UREndEffector.ROBOTIQ2FINGERGRIPPER: + gripper_slot = Slot( + resource_name="robotiq_finger_gripper", + resource_class="URGripper", + capacity=1, + attributes={ + "gripper_type": "robotiq_finger", + "max_grip_force": 235.0, + "min_grip_position": 0, + "max_grip_position": 255, + "description": "UR Robotiq finger gripper slot", + }, + ) + + self.resource_client.init_template( + resource=gripper_slot, + template_name="robotiq_finger_gripper_slot", + description="Template for UR Robotiq finger gripper slot. Used to track what the gripper is holding.", + required_overrides=["resource_name"], + tags=["ur", "gripper", "slot", "robotiq"], + created_by=self.node_definition.node_id, + version="1.0.0", + ) + + # Initialize gripper resource from template + self.end_effector_resource_id = self.resource_client.create_resource_from_template( + template_name="robotiq_finger_gripper_slot", + resource_name=f"ur_gripper_{self.node_definition.node_name}", + add_to_database=True, + ) + elif self.config.end_effector == UREndEffector.PIPETTE: + pipette_pool = Pool( + resource_name="tricontinent_pipette", + resource_class="URPipette", + capacity=1000.0, + attributes={ + "pipette_type": "tricontinent", + "min_volume": 1.0, + "max_volume": 1000.0, + "default_speed": 150, + "description": "Tricontinent pipette pool for tracking tips and aspirated liquid", + }, + ) + + self.resource_client.init_template( + resource=pipette_pool, + template_name="tricontinent_pipette_pool", + description="Template for Tricontinent pipette pool. Tracks pipette tips and aspirated liquids.", + required_overrides=["resource_name"], + tags=["ur", "pipette", "pool", "liquid-handling"], + created_by=self.node_definition.node_id, + version="1.0.0", + ) + # Initialize pipette resource from template + self.end_effector_resource_id = self.resource_client.create_resource_from_template( + template_name="tricontinent_pipette_pool", + resource_name=f"ur_pipette_{self.node_definition.node_name}", + add_to_database=True, + ) def shutdown_handler(self) -> None: """Called to shutdown the node. Should be used to close connections to devices or release any other resources.""" try: self.logger.log("Shutting down") - self.ur_interface.disconnect() + self.integrated_controller.ur_controller.disconnect() self.shutdown_has_run = True - del self.ur_interface - self.ur_interface = None + del self.integrated_controller + self.integrated_controller = None self.logger.log("Shutdown complete.") except Exception as err: self.logger.log_error(f"Error shutting down the UR Node: {err}") @@ -138,21 +132,21 @@ def shutdown_handler(self) -> None: def status_handler(self): """Periodically called to update the current status of the node.""" if not self.node_status.busy: - if self.ur_interface: + if self.integrated_controller: # Getting robot state - self.ur_interface.ur_dashboard.get_overall_robot_status() - movement_state, self.current_location = self.ur_interface.get_movement_state() + self.integrated_controller.ur_dashboard.get_overall_robot_status() + movement_state, self.current_location = self.integrated_controller.ur_controller.get_movement_state() else: self.logger.log_error("UR interface is not initialized") return - if "PROTECTIVE_STOP" in self.ur_interface.ur_dashboard.safety_status: + if "PROTECTIVE_STOP" in self.integrated_controller.ur_dashboard.safety_status: self.node_status.stopped = True self.logger.log_error("UR is in PROTECTIVE_STOP") - if "NORMAL" not in self.ur_interface.ur_dashboard.safety_status: + if "NORMAL" not in self.integrated_controller.ur_dashboard.safety_status: self.node_status.errored = True - self.logger.log_error(f"UR ERROR: {self.ur_interface.ur_dashboard.safety_status}") + self.logger.log_error(f"UR ERROR: {self.integrated_controller.ur_dashboard.safety_status}") else: self.node_status.errored = False self.node_status.stopped = False @@ -168,110 +162,23 @@ def status_handler(self): def state_handler(self) -> None: """Periodically called to update the current state of the node.""" - if self.ur_interface: + if self.integrated_controller: self.node_state = { "current_joint_angles": self.current_location, } - @action(name="getj", description="Get joint angles") - def getj(self): - """Get joint positions""" - joints = self.ur_interface.ur_connection.getj() - self.logger.log_info(joints) - return ActionSucceeded(data={"joints": joints}) - - @action(name="getl", description="Get linear positions") - def getl(self): - """Get linear position""" - lin_pos = self.ur_interface.ur_connection.getl() - self.logger.log_info(lin_pos) - return ActionSucceeded(data={"lin_pos": lin_pos}) - - @action(name="set_freedrive", description="Free robot joints") - def set_freedrive(self, timeout: Annotated[int, "how long to do freedrive"] = 60): - """set the robot into freedrive""" - self.ur_interface.ur_connection.set_freedrive(True, timeout) - - @action(name="set_movement_params", description="Set speed and acceleration parameters") - def set_movement_params( - self, - tcp_pose: Optional[list] = None, - velocity: Optional[float] = None, - acceleration: Optional[float] = None, - gripper_speed: Optional[int] = None, - gripper_force: Optional[int] = None, - ): - """Configure the robot's movement parameters for subsequent transfers""" - if tcp_pose is not None: - self.ur_interface.ur_connection.set_tcp(tcp_pose) - if velocity is not None: - self.ur_interface.velocity = velocity - if acceleration is not None: - self.ur_interface.acceleration = acceleration - if gripper_speed is not None: - self.ur_interface.gripper_speed = gripper_speed - if gripper_force is not None: - self.ur_interface.gripper_force = gripper_force - - @action(name="movej", description="Move the robot using joint angles") - def movej( - self, - joints: Annotated[LocationArgument, "Joint angles to move to"], - acceleration: Annotated[Optional[float], "Acceleration"] = 0.6, - velocity: Annotated[Optional[float], "Velocity"] = 0.6, - ): - """Move the robot using joint angles""" - try: - self.logger.log(f"Move joints: {joints.representation}") - self.ur_interface.ur_connection.movej(joints=joints.representation, acc=acceleration, vel=velocity) - - except Exception as err: - self.logger.log_error(err) - - @action(name="movel", description="Move the robot using linar motion") - def movel( + @action() + def move( self, target: Annotated[LocationArgument, "Linear location to move to"], - acceleration: Annotated[Optional[float], "Acceleration"] = 0.6, - velocity: Annotated[Optional[float], "Velocity"] = 0.6, - joint_angle_locations: Annotated[bool, "Use joint angles for the location"] = True, + linear_motion: Annotated[bool, "Use linear motion"] = True, ): - """Move the robot using linear motion""" - try: - self.logger.log(f"Move location: {target.representation}") - if joint_angle_locations: - target.representation = get_pose_from_joint_angles(target.representation) - self.ur_interface.ur_connection.movel(tpose=target.representation, acc=acceleration, vel=velocity) + """Move the robot to target location""" - except Exception as err: - self.logger.log_error(err) + self.logger.log(f"Move location: {target.representation}") + self.integrated_controller.ur_controller.move_to_location(location=target, linear_motion=linear_motion) - @action(name="toggle_gripper", description="Move the robot gripper") - def toggle_gripper( - self, - open: Annotated[bool, "Open?"] = False, - close: Annotated[bool, "Close?"] = False, - ): - """Open or close the robot gripper.""" - gripper = FingerGripperController(hostname=self.config.ur_ip, ur=self.ur_interface, logger=self.logger) - self.logger.info("Toggle gripper: Connecting to gripper...") - gripper.connect_gripper() - self.logger.info("Toggle gripper: Gripper connected") - if open: - gripper.open_gripper() - self.logger.info("Toggle gripper: Gripper opened") - elif close: - gripper.close_gripper() - self.logger.info("Toggle gripper: Gripper closed") - else: - self.logger.info("Toggle gripper: No action taken") - gripper.disconnect_gripper() - self.logger.info("Toggle gripper: Gripper disconnected") - - @action( - name="gripper_transfer", - description="Execute a transfer in between source and target locations using Robotiq grippers", - ) + @action def gripper_transfer( self, home: Annotated[Union[LocationArgument, list], "Home location"], @@ -281,542 +188,61 @@ def gripper_transfer( target_approach_axis: Annotated[Optional[str], "Source location approach axis, (X/Y/Z)"] = "z", source_approach_distance: Annotated[Optional[float], "Approach distance in meters"] = 0.05, target_approach_distance: Annotated[Optional[float], "Approach distance in meters"] = 0.05, - gripper_open: Annotated[Optional[int], "Set a max value for the gripper open state"] = 0, - gripper_close: Annotated[Optional[int], "Set a min value for the gripper close state"] = 255, - joint_angle_locations: Annotated[bool, "Use joint angles for all the locations"] = True, ): """Make a transfer using the finger gripper. This function uses linear motions to perform the pick and place movements.""" - try: - if ( - self.config.use_resources - and isinstance(source, LocationArgument) - and isinstance(target, LocationArgument) - ): - self.ur_interface.tool_resource_id = self.gripper_resource.resource_id - source_resource = self.resource_client.get_resource(source.resource_id) - target_resource = self.resource_client.get_resource(target.resource_id) - if source_resource.quantity == 0: - raise ValueError("Resource manager: Source location is empty!") - if target_resource.quantity != 0: - raise ValueError("Resource manager: Target is occupied!") - - if joint_angle_locations and isinstance(source, LocationArgument) and isinstance(target, LocationArgument): - source.representation = get_pose_from_joint_angles( - joints=source.representation, robot_model=self.config.ur_model - ) - target.representation = get_pose_from_joint_angles( - joints=target.representation, robot_model=self.config.ur_model - ) - elif joint_angle_locations and isinstance(source, list) and isinstance(target, list): - source = get_pose_from_joint_angles(joints=source, robot_model=self.config.ur_model) - target = get_pose_from_joint_angles(joints=target, robot_model=self.config.ur_model) - - self.ur_interface.gripper_transfer( - home=home, - source=source, - target=target, - source_approach_distance=source_approach_distance, - target_approach_distance=target_approach_distance, - source_approach_axis=source_approach_axis, - target_approach_axis=target_approach_axis, - gripper_open=gripper_open, - gripper_close=gripper_close, - ) - - except GripperError as err: - self.logger.log_error(f"Gripper error during transfer: {err}") - return ActionFailed(errors=str(err)) - except URMovementError as err: - self.logger.log_error(f"Movement error during transfer: {err}") - return ActionFailed(errors=str(err)) - - except Exception as err: - self.logger.log_error(f"Unexpected error during gripper transfer: {err}") - return ActionFailed(errors=str(err)) + self.gripper_pick(home, source, source_approach_axis, source_approach_distance) + self.gripper_place(home, target, target_approach_axis, target_approach_distance) - @action() + @action def gripper_pick( self, home: Annotated[LocationArgument, "Home location"], source: Annotated[LocationArgument, "Location to transfer sample from"], source_approach_axis: Annotated[Optional[str], "Source location approach axis, (X/Y/Z)"] = "z", source_approach_distance: Annotated[Optional[float], "Approach distance in meters"] = 0.05, - gripper_close: Annotated[Optional[int], "Set a min value for the gripper close state"] = 255, - joint_angle_locations: Annotated[bool, "Use joint angles for all the locations"] = True, ): """Use the gripper to pick a piece of labware from the specified source""" - try: - if self.config.use_resources: - self.ur_interface.tool_resource_id = self.gripper_resource.resource_id - source_resource = self.resource_client.get_resource(source.resource_id) - if source_resource.quantity == 0: - raise ValueError("Resource manager: Source location is empty!") - - if joint_angle_locations and isinstance(source, LocationArgument): - source.representation = get_pose_from_joint_angles( - joints=source.representation, robot_model=self.config.ur_model - ) - elif joint_angle_locations and isinstance(source, list): - source = get_pose_from_joint_angles(joints=source, robot_model=self.config.ur_model) - self.logger.log_info(f"Picking from source: {source.representation}") - self.ur_interface.gripper_pick( - home=home, - source=source, - source_approach_distance=source_approach_distance, - source_approach_axis=source_approach_axis, - gripper_close=gripper_close, - ) - - except GripperError as err: - self.logger.log_error(f"Gripper error during pick: {err}") - return ActionFailed(errors=str(err)) - - except URMovementError as err: - self.logger.log_error(f"Movement error during pick: {err}") - return ActionFailed(errors=str(err)) + self.logger.log_info(f"Picking from source: {source.representation}") - except Exception as err: - self.logger.log_error(f"Unexpected error during gripper pick: {err}") - return ActionFailed(errors=str(err)) + self.integrated_controller.gripper_pick( + home=home, + source=source, + source_approach_distance=source_approach_distance, + source_approach_axis=source_approach_axis, + ) - @action() + @action def gripper_place( self, home: Annotated[LocationArgument, "Home location"], target: Annotated[LocationArgument, "Location to transfer sample to"], target_approach_axis: Annotated[Optional[str], "Source location approach axis, (X/Y/Z)"] = "z", target_approach_distance: Annotated[Optional[float], "Approach distance in meters"] = 0.05, - gripper_open: Annotated[Optional[int], "Set a max value for the gripper open state"] = 0, - joint_angle_locations: Annotated[bool, "Use joint angles for all the locations"] = True, ): """Use the gripper to place a piece of labware at the target.""" - try: - if self.config.use_resources and isinstance(target, LocationArgument): - self.ur_interface.tool_resource_id = self.gripper_resource.resource_id - target_resource = self.resource_client.get_resource(target.resource_id) - if target_resource.quantity != 0: - raise ValueError("Resource manager: Target is occupied!") - - if joint_angle_locations and isinstance(target, LocationArgument): - target.representation = get_pose_from_joint_angles( - joints=target.representation, robot_model=self.config.ur_model - ) - elif joint_angle_locations and isinstance(target, list): - target = get_pose_from_joint_angles(joints=target, robot_model=self.config.ur_model) - - self.ur_interface.gripper_place( - home=home, - target=target, - target_approach_distance=target_approach_distance, - target_approach_axis=target_approach_axis, - gripper_open=gripper_open, - ) - except GripperError as err: - self.logger.log_error(f"Gripper error during place: {err}\n{traceback.format_exc()}") - return ActionFailed(errors=str(err)) - - except URMovementError as err: - self.logger.log_error(f"Movement error during place: {err}\n{traceback.format_exc()}") - return ActionFailed(errors=str(err)) - - except Exception as err: - self.logger.log_error(f"Unexpected error during gripper place: {err}\n{traceback.format_exc()}") - return ActionFailed(errors=str(err)) - - @action( - name="pick_tool", - description="Picks up a tool using the provided tool location", - ) - def pick_tool( - self, - home: Annotated[Union[LocationArgument, list], "Home location"], - tool_loc: Annotated[Union[LocationArgument, list], "Tool location"], - docking_axis: Annotated[Optional[str], "Docking axis, (X/Y/Z)"] = "y", - payload: Annotated[Optional[float], "Tool payload"] = None, - tool_name: Annotated[Optional[str], "Tool name)"] = None, - joint_angle_locations: Annotated[bool, "Use joint angles for all the locations"] = True, - ): - """Pick a tool with the UR""" - if joint_angle_locations and isinstance(tool_loc, LocationArgument): - tool_loc.representation = get_pose_from_joint_angles( - joints=tool_loc.representation, robot_model=self.config.ur_model - ) - elif joint_angle_locations and isinstance(tool_loc, list): - tool_loc = get_pose_from_joint_angles(joints=tool_loc, robot_model=self.config.ur_model) - - self.ur_interface.pick_tool( - home=home, - tool_loc=tool_loc, - docking_axis=docking_axis, - payload=payload, - tool_name=tool_name, - ) - - @action(name="Place_tool", description="Places the attached tool back to the provided tool docking location") - def place_tool( - self, - home: Annotated[Union[LocationArgument, list], "Home location"], - tool_docking: Annotated[Union[LocationArgument, list], "Tool docking location"], - docking_axis: Annotated[Optional[str], "Docking axis, (X/Y/Z)"] = "y", - tool_name: Annotated[Optional[str], "Tool name)"] = None, - joint_angle_locations: Annotated[bool, "Use joint angles for all the locations"] = True, - ): - """Place a tool with the UR""" - if joint_angle_locations and isinstance(tool_docking, LocationArgument): - tool_docking.representation = get_pose_from_joint_angles( - joints=tool_docking.representation, robot_model=self.config.ur_model - ) - elif joint_angle_locations and isinstance(tool_docking, list): - tool_docking = get_pose_from_joint_angles(joints=tool_docking, robot_model=self.config.ur_model) - - self.ur_interface.place_tool( - home=home, - tool_loc=tool_docking, - docking_axis=docking_axis, - tool_name=tool_name, - ) - - @action( - name="gripper_screw_transfer", - description="Performs a screw transfer using the Robotiq gripper and custom screwdriving bits", - ) - def gripper_screw_transfer( - self, - home: Annotated[Union[LocationArgument, list], "Home location"], - screwdriver_loc: Annotated[Union[LocationArgument, list], "Screwdriver location"], - screw_loc: Annotated[Union[LocationArgument, list], "Screw location"], - target: Annotated[Union[LocationArgument, list], "Location where the srewdriving will be performed"], - screw_time: Annotated[Optional[int], "Srew time in seconds"] = 9, - gripper_open: Annotated[Optional[int], "Set a max value for the gripper open state"] = 0, - gripper_close: Annotated[Optional[int], "Set a min value for the gripper close state"] = 255, - joint_angle_locations: Annotated[bool, "Use joint angles for all the locations"] = True, - ): - """Make a screwdriving transfer using Robotiq gripper and custom screwdriving bits with UR""" - - if joint_angle_locations and isinstance(screwdriver_loc, LocationArgument): - screwdriver_loc.representation = get_pose_from_joint_angles( - joints=screwdriver_loc.representation, robot_model=self.config.ur_model - ) - screw_loc.representation = get_pose_from_joint_angles( - joints=screw_loc.representation, robot_model=self.config.ur_model - ) - target.representation = get_pose_from_joint_angles( - joints=target.representation, robot_model=self.config.ur_model - ) - elif joint_angle_locations and isinstance(screwdriver_loc, list): - screwdriver_loc = get_pose_from_joint_angles(joints=screwdriver_loc, robot_model=self.config.ur_model) - screw_loc = get_pose_from_joint_angles(joints=screw_loc, robot_model=self.config.ur_model) - target = get_pose_from_joint_angles(joints=target, robot_model=self.config.ur_model) - self.ur_interface.gripper_screw_transfer( + self.integrated_controller.gripper_place( home=home, - screwdriver_loc=screwdriver_loc, - screw_loc=screw_loc, - screw_time=screw_time, target=target, - gripper_open=gripper_open, - gripper_close=gripper_close, - ) - - @action( - name="pipette_transfer", - description="Make a pipette transfer to transfer sample liquids in between two locations", - ) - def pipette_transfer( - self, - home: Annotated[Union[LocationArgument, list], "Home location"], - source: Annotated[Union[LocationArgument, list], "Initial location of the sample"], - target: Annotated[Union[LocationArgument, list], "Target location of the sample"], - tip_loc: Annotated[Union[LocationArgument, list], "New tip location"], - tip_trash: Annotated[Union[LocationArgument, list], "Tip trash location"], - volume: Annotated[float, "Set a volume in micro liters"], - joint_angle_locations: Annotated[bool, "Use joint angles for all the locations"] = True, - ): - """Make a pipette transfer for the defined volume with UR""" - - if joint_angle_locations and isinstance(source, LocationArgument): - source.representation = get_pose_from_joint_angles( - joints=source.representation, robot_model=self.config.ur_model - ) - target.representation = get_pose_from_joint_angles( - joints=target.representation, robot_model=self.config.ur_model - ) - tip_loc.representation = get_pose_from_joint_angles( - joints=tip_loc.representation, robot_model=self.config.ur_model - ) - tip_trash.representation = get_pose_from_joint_angles( - joints=tip_trash.representation, robot_model=self.config.ur_model - ) - elif joint_angle_locations and isinstance(source, list): - source = get_pose_from_joint_angles(joints=source, robot_model=self.config.ur_model) - target = get_pose_from_joint_angles(joints=target, robot_model=self.config.ur_model) - tip_loc = get_pose_from_joint_angles(joints=tip_loc, robot_model=self.config.ur_model) - tip_trash = get_pose_from_joint_angles(joints=tip_trash, robot_model=self.config.ur_model) - - self.ur_interface.pipette_transfer( - home=home, - tip_loc=tip_loc, - tip_trash=tip_trash, - source=source, - target=target, - volume=volume, - ) - - @action( - name="pipette_pick_and_move_sample", - description="Picks and moves a sample using the pipette", - ) - def pipette_pick_and_move_sample( - self, - home: Annotated[Union[LocationArgument, list], "Home location in joint angles"], - sample_loc: Annotated[Union[LocationArgument, list], "Sample location"], - target: Annotated[Union[LocationArgument, list], "Location of the object"], - volume: Annotated[int, "Set a volume in micro liters"] = 10, - safe_waypoint: Annotated[Union[LocationArgument, list], "Safe waypoint in joint angles"] = None, - tip_loc: Annotated[Union[LocationArgument, list], "Tip location"] = None, - joint_angle_locations: Annotated[bool, "Use joint angles for all the locations"] = True, - pipette_speed: Annotated[Optional[int], "Pipette speed in m/s"] = 150, - ): - """Picks and moves a sample with UR""" - - if joint_angle_locations and isinstance(target, LocationArgument): - target.representation = get_pose_from_joint_angles( - joints=target.representation, robot_model=self.config.ur_model - ) - sample_loc.representation = get_pose_from_joint_angles( - joints=sample_loc.representation, robot_model=self.config.ur_model - ) - tip_loc.representation = ( - get_pose_from_joint_angles(joints=tip_loc.representation, robot_model=self.config.ur_model) - if tip_loc - else None - ) - elif joint_angle_locations and isinstance(target, list): - target = get_pose_from_joint_angles(joints=target, robot_model=self.config.ur_model) - sample_loc = get_pose_from_joint_angles(joints=sample_loc, robot_model=self.config.ur_model) - tip_loc = get_pose_from_joint_angles(joints=tip_loc, robot_model=self.config.ur_model) if tip_loc else None - - self.ur_interface.tool_resource_id = self.pipette_resource.resource_id - - self.ur_interface.pipette_pick_and_move_sample( - home=home, - safe_waypoint=safe_waypoint, - sample_loc=sample_loc, - target=target, - volume=volume, - tip_loc=tip_loc, - pipette_speed=pipette_speed, - ) - - @action( - name="pipette_dispense_and_retrieve", - description="Dispenses a sample and retrieves the pipette tip", - ) - def pipette_dispense_and_retrieve( - self, - home: Annotated[Union[LocationArgument, list], "Home location in joint angles"], - target: Annotated[Union[LocationArgument, list], "Location of the object"], - volume: Annotated[int, "Set a volume in micro liters"] = 10, - safe_waypoint: Annotated[Union[LocationArgument, list], "Safe waypoint in joint angles"] = None, - tip_trash: Annotated[Union[LocationArgument, list], "Tip trash location"] = None, - joint_angle_locations: Annotated[bool, "Use joint angles for all the locations"] = True, - pipette_speed: Annotated[Optional[int], "Pipette speed in m/s"] = 150, - ): - """Dispenses a sample and retrieves the pipette with UR""" - if joint_angle_locations and isinstance(target, LocationArgument): - target.representation = get_pose_from_joint_angles( - joints=target.representation, robot_model=self.config.ur_model - ) - tip_trash.representation = ( - get_pose_from_joint_angles(joints=tip_trash.representation, robot_model=self.config.ur_model) - if tip_trash - else None - ) - elif joint_angle_locations and isinstance(target, list): - target = get_pose_from_joint_angles(joints=target, robot_model=self.config.ur_model) - tip_trash = ( - get_pose_from_joint_angles(joints=tip_trash, robot_model=self.config.ur_model) if tip_trash else None - ) - - self.ur_interface.tool_resource_id = self.pipette_resource.resource_id - - self.ur_interface.pipette_dispense_and_retrieve( - home=home, - safe_waypoint=safe_waypoint, - target=target, - volume=volume, - tip_trash=tip_trash, - pipette_speed=pipette_speed, - ) - - @action( - name="pick_and_flip_object", - description="Picks and flips an object 180 degrees", - ) - def pick_and_flip_object( - self, - home: Annotated[Union[LocationArgument, list], "Home location"], - target: Annotated[Union[LocationArgument, list], "Location of the object"], - approach_axis: Annotated[Optional[str], "Approach axis, (X/Y/Z)"] = "z", - target_approach_distance: Annotated[Optional[float], "Approach distance in meters"] = 0.05, - gripper_open: Annotated[Optional[int], "Set a max value for the gripper open state"] = 0, - gripper_close: Annotated[Optional[int], "Set a min value for the gripper close state"] = 255, - joint_angle_locations: Annotated[bool, "Use joint angles for all the locations"] = True, - ): - """Picks and flips an object 180 degrees with UR""" - - if joint_angle_locations and isinstance(target, LocationArgument): - target.representation = get_pose_from_joint_angles( - joints=target.representation, robot_model=self.config.ur_model - ) - elif joint_angle_locations and isinstance(target, list): - target = get_pose_from_joint_angles(joints=target, robot_model=self.config.ur_model) - - self.ur_interface.tool_resource_id = self.gripper_resource.resource_id - - self.ur_interface.pick_and_flip_object( - home=home, - target=target, - approach_axis=approach_axis, target_approach_distance=target_approach_distance, - gripper_open=gripper_open, - gripper_close=gripper_close, - ) - - @action( - name="remove_cap", - description="Removes caps from sample vials", - ) - def remove_cap( - self, - home: Annotated[Union[LocationArgument, list], "Home location"], - source: Annotated[Union[LocationArgument, list], "Location of the vial cap"], - target: Annotated[ - Union[LocationArgument, list], - "Location of where the cap will be placed after it is removed from the vail", - ], - gripper_open: Annotated[Optional[int], "Set a max value for the gripper open state"] = 0, - gripper_close: Annotated[Optional[int], "Set a min value for the gripper close state"] = 255, - joint_angle_locations: Annotated[bool, "Use joint angles for all the locations"] = True, - ): - """Remove caps from sample vials with UR""" - if joint_angle_locations and isinstance(source, LocationArgument): - source.representation = get_pose_from_joint_angles( - joints=source.representation, robot_model=self.config.ur_model - ) - target.representation = get_pose_from_joint_angles( - joints=target.representation, robot_model=self.config.ur_model - ) - elif joint_angle_locations and isinstance(source, list): - source = get_pose_from_joint_angles(joints=source, robot_model=self.config.ur_model) - target = get_pose_from_joint_angles(joints=target, robot_model=self.config.ur_model) - - self.ur_interface.tool_resource_id = self.gripper_resource.resource_id - - self.ur_interface.remove_cap( - home=home, - source=source, - target=target, - gripper_open=gripper_open, - gripper_close=gripper_close, - ) - - @action( - name="place_cap", - description="Places caps back to sample vials", - ) - def place_cap( - self, - home: Annotated[Union[LocationArgument, list], "Home location"], - source: Annotated[Union[LocationArgument, list], "Vail cap initial location"], - target: Annotated[Union[LocationArgument, list], "The vail location where the cap will installed"], - gripper_open: Annotated[Optional[int], "Set a max value for the gripper open state"] = 0, - gripper_close: Annotated[Optional[int], "Set a min value for the gripper close state"] = 255, - joint_angle_locations: Annotated[bool, "Use joint angles for all the locations"] = True, - ): - """Places caps back to sample vials with UR""" - if joint_angle_locations and isinstance(source, LocationArgument): - source.representation = get_pose_from_joint_angles( - joints=source.representation, robot_model=self.config.ur_model - ) - target.representation = get_pose_from_joint_angles( - joints=target.representation, robot_model=self.config.ur_model - ) - elif joint_angle_locations and isinstance(source, list): - source = get_pose_from_joint_angles(joints=source, robot_model=self.config.ur_model) - target = get_pose_from_joint_angles(joints=target, robot_model=self.config.ur_model) - - self.ur_interface.tool_resource_id = self.gripper_resource.resource_id - - self.ur_interface.place_cap( - home=home, - source=source, - target=target, - gripper_open=gripper_open, - gripper_close=gripper_close, - ) - - @action( - name="run_urp_program", - description="Runs a URP program on the UR", - ) - def run_urp_program( - self, - transfer_file_path=Annotated[str, "Transfer file path"], - program_name=Annotated[str, "Program name"], - ): - """Run an URP program on the UR""" - self.ur_interface.run_urp_program( - transfer_file_path=transfer_file_path, - program_name=program_name, + target_approach_axis=target_approach_axis, ) - @action( - name="set_digital_io", - description="Sets a channel IO output on the UR", - ) - def set_digital_io( - self, - channel=Annotated[int, "Channel number"], - value=Annotated[bool, "True/False"], - ): - """Sets a channel IO output on the UR""" - self.ur_interface.set_digital_io(channel=channel, value=value) - - def get_location(self) -> AdminCommandResponse: - """Return the current position of the ur robot""" - return AdminCommandResponse(data=self.ur_interface.ur_connection.getj()) - def reset(self) -> AdminCommandResponse: """Reset the ur robot""" self.logger.log("Resetting node...") # If resetting startup handler does not work, try re-initializing the dashboard - self.ur_interface.disconnect() + self.integrated_controller.disconnect() result = super().reset() self.logger.log("Node reset.") return result - @action(name="e_stop", description="Emergency stop the UR robot") - def e_stop(self): - """Emergency stop the UR robot""" - try: - self.ur_interface.ur_dashboard.power_off() - self.logger.log_info("EMERGENCY STOP EXECUTED") - - except Exception as err: - self.logger.log_error(f"FAILED EMERGENCY STOP: {err}") - return ActionFailed(errors=str(err)) - def safety_stop(self) -> AdminCommandResponse: """Safety stop the UR robot""" - try: - self.ur_interface.ur_dashboard.stop_program() - self.logger.log_info("SAFETY STOP EXECUTED") - return AdminCommandResponse(success=True) - except Exception as err: - self.logger.log_error(f"FAILED SAFETY STOP: {err}") - return AdminCommandResponse(success=False, errors=str(err)) + self.integrated_controller.ur_dashboard.stop_program() + self.logger.log_info("SAFETY STOP EXECUTED") + return AdminCommandResponse(success=True) if __name__ == "__main__":