From 31075bb207be385be1640b4ab3dd1a46cac2330e Mon Sep 17 00:00:00 2001 From: rproesma Date: Thu, 21 Nov 2024 11:29:31 +0100 Subject: [PATCH] Added schunk stop and move_relative functions --- .../airo_robots/grippers/hardware/readme.md | 14 +++--- .../grippers/hardware/schunk_egk40_usb.py | 49 ++++++++++++++++--- 2 files changed, 49 insertions(+), 14 deletions(-) diff --git a/airo-robots/airo_robots/grippers/hardware/readme.md b/airo-robots/airo_robots/grippers/hardware/readme.md index 66fdd81..707b2ba 100644 --- a/airo-robots/airo_robots/grippers/hardware/readme.md +++ b/airo-robots/airo_robots/grippers/hardware/readme.md @@ -1,14 +1,16 @@ ## For Schunk Gripper with RS-485 physical connection -1. git clone https://github.com/SCHUNK-SE-Co-KG/bkstools.git +1. `git clone https://github.com/SCHUNK-SE-Co-KG/bkstools.git` -2. follow the bkstools' installing step (get .whl source as bkstools) +2. Follow the bkstools' installing step (get .whl source as bkstools) -3. find the ID for schunk: python ./bkstools/scripts/bks_scan.py +3. Find the name of the USB interface where your Schunk is connected. Run `sudo dmesg | grep tty` and you should see a line like `ch341-uart converter now attached to ttyUSB0`. In this case, the USB interface is ttyUSB0. -4. change the defualt ID in ./bkstools/bks_lib/bks_base.py (change slave_id to the scan_id) +4. Find the ID of the Schunk gripper: run `python ./bkstools/scripts/bks_scan.py -H /dev/ttyUSB*` with `*` the appropriate index. -testing demo: python ./bkstools/demo/demo_bks_grip_outside_inside.py +5. Change slave_id (line 30) in ./bkstools/bks_lib/bks_base.py to the ID you found in step 4. + +6. Testing demo `python ./bkstools/demo/demo_bks_grip_outside_inside.py` ## Debugging @@ -16,4 +18,4 @@ If the Schunk doesn't show up as /dev/ttyUSB*, but `lsusb` lists `QinHeng Electr `sudo dmesg | grep brltty` shows `usbfs: interface 0 claimed by ch341 while 'brltty' sets config #1`, then: `sudo apt remove brltty`, replug the usb cable, the Schunk should now show up as /dev/ttyUSB*. -Maybe necessary but unconfirmed: `sudo apt-get install build-essential linux-source` +Could be necessary (unconfirmed): `sudo apt-get install build-essential linux-source` diff --git a/airo-robots/airo_robots/grippers/hardware/schunk_egk40_usb.py b/airo-robots/airo_robots/grippers/hardware/schunk_egk40_usb.py index f98b465..4506233 100644 --- a/airo-robots/airo_robots/grippers/hardware/schunk_egk40_usb.py +++ b/airo-robots/airo_robots/grippers/hardware/schunk_egk40_usb.py @@ -3,6 +3,7 @@ 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 filelock import AsyncWindowsFileLock from pyschunk.generated.generated_enums import eCmdCode from typing import Optional from airo_robots.awaitable_action import AwaitableAction @@ -25,6 +26,8 @@ def __init__(self, usb_interface="/dev/ttyUSB0", 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. + TODO: this class currently doesn't support fingertips that, when fully opened, would have a larger width than + the Schunk's maximum width of 83mm. Such fingertips would not touch when the Schunk is "closed". """ super().__init__(self.SCHUNK_DEFAULT_SPECS) if fingers_max_stroke == -1: @@ -87,7 +90,7 @@ def get_current_width(self) -> float: 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): + force: Optional[float] = SCHUNK_DEFAULT_SPECS.min_force) -> AwaitableAction: """ Moves the gripper to a certain position at a certain speed with a certain force :param width: in m @@ -96,18 +99,36 @@ def move(self, width: float, speed: Optional[float] = SCHUNK_DEFAULT_SPECS.min_s """ self.speed = speed self.max_grasp_force = force + _width = np.clip(width, self.gripper_specs.min_width, self.gripper_specs.max_width) # Reasoning: - # width_without_fingers = width + self.width_loss_due_to_fingers + # 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.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): + def move_relative(self, width_difference: float, speed: Optional[float] = SCHUNK_DEFAULT_SPECS.min_speed, + force: Optional[float] = SCHUNK_DEFAULT_SPECS.min_force) -> AwaitableAction: + """ + Moves the gripper to a certain position at a certain speed with a certain force + :param width_difference: in m, a positive difference will make the gripper open, a negative difference makes it close + :param speed: in m/s + :param force: in N + """ + self.speed = speed + self.max_grasp_force = force + self.bksb.set_pos = -width_difference*1000 + self.bksb.command_code = eCmdCode.MOVE_POS_REL + + def move_done_condition() -> bool: + return self.current_speed == 0 + return AwaitableAction(move_done_condition) + + def grip(self) -> AwaitableAction: """ 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. @@ -124,7 +145,19 @@ def move_done_condition() -> bool: return self.current_speed == 0 return AwaitableAction(move_done_condition) - def calibrate_width(self): + def stop(self) -> None: + """ + TODO: figure out difference with fast_stop() + """ + self.bksb.command_code = eCmdCode.CMD_STOP + + def fast_stop(self) -> None: + """ + TODO: figure out difference with stop() + """ + self.bksb.command_code = eCmdCode.CMD_FAST_STOP + + def calibrate_width(self) -> None: """ 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 @@ -136,8 +169,8 @@ def calibrate_width(self): 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): + def input(self, prompt) -> None: return keep_communication_alive_input(self.bksb, prompt) - def sleep(self, t): - return keep_communication_alive_sleep(self.bksb, t) + def sleep(self, duration) -> None: + return keep_communication_alive_sleep(self.bksb, duration)