Added schunk stop and move_relative functions

airo-robots/airo_robots/grippers/hardware/readme.md

@@ -1,19 +1,21 @@
 ## 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
 
 If the Schunk doesn't show up as /dev/ttyUSB*, but `lsusb` lists `QinHeng Electronics CH340 serial converter`, and
 `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`

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)