From 32e7aa16497f6f86c809bd55cecfe079691d6ef3 Mon Sep 17 00:00:00 2001 From: rproesma Date: Tue, 12 Nov 2024 14:08:50 +0100 Subject: [PATCH] added SchunkEGK40_USB class --- .../grippers/hardware/schunkEGK40_usb.py | 143 ++++++++++++++++++ 1 file changed, 143 insertions(+) create mode 100644 airo-robots/airo_robots/grippers/hardware/schunkEGK40_usb.py diff --git a/airo-robots/airo_robots/grippers/hardware/schunkEGK40_usb.py b/airo-robots/airo_robots/grippers/hardware/schunkEGK40_usb.py new file mode 100644 index 0000000..f98b465 --- /dev/null +++ b/airo-robots/airo_robots/grippers/hardware/schunkEGK40_usb.py @@ -0,0 +1,143 @@ +import time + +import numpy as np +from bkstools.bks_lib.bks_base import BKSBase, keep_communication_alive_input, keep_communication_alive_sleep +from bkstools.scripts.bks_grip import WaitGrippedOrError +from pyschunk.generated.generated_enums import eCmdCode +from typing import Optional +from airo_robots.awaitable_action import AwaitableAction +from airo_robots.grippers.parallel_position_gripper import ParallelPositionGripper, ParallelPositionGripperSpecs + + +def rescale_range(x: float, from_min: float, from_max: float, to_min: float, to_max: float) -> float: + return to_min + (x - from_min) / (from_max - from_min) * (to_max - to_min) + + +class SchunkEGK40_USB(ParallelPositionGripper): + # values obtained from https://schunk.com/be/nl/grijpsystemen/parallelgrijper/egk/egk-40-mb-m-b/p/000000000001491762 + SCHUNK_DEFAULT_SPECS = ParallelPositionGripperSpecs(0.083, 0.0, 150, 55, 0.0575, 0.0055) + + def __init__(self, usb_interface="/dev/ttyUSB0", + fingers_max_stroke: Optional[float] = None) -> None: + """ + usb_interface: the USB interface to which the gripper is connected. + + fingers_max_stroke: + allow for custom max stroke width (if you have fingertips that are closer together). If None, the Schunk will + calibrate itself. If -1, the default values are kept. + """ + super().__init__(self.SCHUNK_DEFAULT_SPECS) + if fingers_max_stroke == -1: + # keep default width settings + pass + elif fingers_max_stroke: + self._gripper_specs.max_width = fingers_max_stroke + elif fingers_max_stroke is None: + self.calibrate_width() + self.width_loss_due_to_fingers = self.SCHUNK_DEFAULT_SPECS.max_width - self.gripper_specs.max_width + + # The BKSBase object supports communication with the Schunk EGK40 + self.bksb = BKSBase(usb_interface) # , debug=args.debug, repeater_timeout=args.repeat_timeout, + # repeater_nb_tries=args.repeat_nb_tries) + # Prepare gripper: Acknowledge any pending error: + self.bksb.command_code = eCmdCode.CMD_ACK + time.sleep(0.1) + + @property + def speed(self) -> float: + """returns speed setting [m/s].""" + return self.bksb.set_vel / 1000 + + @speed.setter + def speed(self, new_speed: float) -> None: + """sets the max speed [m/s].""" + _new_speed = np.clip(new_speed, self.gripper_specs.min_speed, self.gripper_specs.max_speed) + self.bksb.set_vel = float(_new_speed * 1000) # value must be set in mm/s for bkstools + + @property + def current_speed(self) -> float: + """returns current speed [m/s].""" + return self.bksb.actual_vel / 1000 + + @property + def max_grasp_force(self) -> float: + _force = rescale_range(self.bksb.set_force, 0, 100, + self.gripper_specs.min_force, self.gripper_specs.max_force) + return _force + + @max_grasp_force.setter + def max_grasp_force(self, new_force: float) -> None: + """sets the max grasping force [N].""" + _new_force = np.clip(new_force, self.gripper_specs.min_force, self.gripper_specs.max_force) + _new_force = rescale_range(_new_force, self.gripper_specs.min_force, self.gripper_specs.max_force, + 0, 100) + self.bksb.set_force = _new_force + + @property + def current_motor_current(self) -> float: + """returns current motor current usage [unit?], this is a proxy for force.""" + return self.bksb.actual_cur + + def get_current_width(self) -> float: + """the current opening of the fingers in meters""" + # Reasoning: + # width_without_fingers = self.SCHUNK_DEFAULT_SPECS.max_width - self.bksb.actual_pos / 1000 + # return width_without_fingers - self.width_loss_due_to_fingers + # The above equates to: + return self.gripper_specs.max_width - (self.bksb.actual_pos / 1000) + + def move(self, width: float, speed: Optional[float] = SCHUNK_DEFAULT_SPECS.min_speed, + force: Optional[float] = SCHUNK_DEFAULT_SPECS.min_force): + """ + Moves the gripper to a certain position at a certain speed with a certain force + :param width: in m + :param speed: in m/s + :param force: in N + """ + self.speed = speed + self.max_grasp_force = force + # Reasoning: + # width_without_fingers = width + self.width_loss_due_to_fingers + # self.bksb.set_pos = (self.SCHUNK_DEFAULT_SPECS.max_width - width_without_fingers) * 1000 + # The above equates to: + self.bksb.set_pos = (self.gripper_specs.max_width - width) * 1000 + self.bksb.command_code = eCmdCode.MOVE_POS + + def move_done_condition() -> bool: + return self.current_speed == 0 + return AwaitableAction(move_done_condition) + + def grip(self): + """ + Moves the gripper until object contact. When using MOVE_FORCE as below, it seems that the force and speed must + be set to 50% and 0 mm/s respectively. + :param speed: in m/s + :param force: in N + """ + self.bksb.set_force = 50 # target force to 50 % => BasicGrip + self.bksb.set_vel = 0.0 # target velocity 0 => BasicGrip + self.bksb.grp_dir = True # grip from outside + self.bksb.command_code = eCmdCode.MOVE_FORCE # (for historic reasons the actual grip command for simple gripping is called MOVE_FORCE...) + WaitGrippedOrError(self.bksb) + + def move_done_condition() -> bool: + return self.current_speed == 0 + return AwaitableAction(move_done_condition) + + def calibrate_width(self): + """ + Robotiq-like calibration to detect the gripper position where the (custom) fingertips touch. Note that the + minimum grasp force of 55N is perhaps already large for custom fingertips, preferably you find the allowable + grasp with for yourself. + TODO: Could monitoring motor current (self.current_motor_current) allow for a more delicate calibration? + """ + self.move(width=self.gripper_specs.max_width, speed=self.gripper_specs.max_speed, force=self.gripper_specs.min_force).wait() + self.grip().wait() + self._gripper_specs.max_width = self.SCHUNK_DEFAULT_SPECS.max_width - self.get_current_width() + self.move(width=self.gripper_specs.max_width, speed=self.gripper_specs.max_speed, force=self.gripper_specs.min_force).wait() + + def input(self, prompt): + return keep_communication_alive_input(self.bksb, prompt) + + def sleep(self, t): + return keep_communication_alive_sleep(self.bksb, t)