airo-robots 2025.4.0__py3-none-any.whl

airo_robots/__init__.py

@@ -0,0 +1 @@
+# keep mypy happy

airo_robots/awaitable_action.py

@@ -0,0 +1,85 @@
+import enum
+import time
+import warnings
+from typing import Callable, Optional
+
+
+class ACTION_STATUS_ENUM(enum.Enum):
+    EXECUTING = 1
+    SUCCEEDED = 2
+    TIMEOUT = 3
+
+
+class AwaitableAction:
+    def __init__(
+        self,
+        termination_condition: Callable[..., bool],
+        default_timeout: float = 30.0,
+        default_sleep_resolution: float = 0.1,
+    ):
+        """
+
+        Args:
+            termination_condition (Callable[..., bool]): Any  callable that returns True when the action is completed.
+            In the simplest case, it can be a lambda that returns True.
+            It can also be true when the gripper is in the desired position.
+            Or it could be true after a certain amount of time has passed since the action was started.
+
+            default_timeout (float, optional): The max waiting time before the wait returns and raises a warning.
+            Select an appropriate default value for the command you are creating this AwaitableAction for.
+
+            default_sleep_resolution (float, optional): The length of the time.sleep() in each iteration.
+            Select an appropriate default value for the command you are creating this AwaitableAction for.
+
+            Note that the scope of this action is to send 1 command, do some other things, then wait for the command to finish.
+            If you send multiple commands to the gripper, there is no guarantee that the gripper will execute them in the order you send them,
+             as the gripper might preempt intermediate commands after it has finished the current command.
+            Such preemption will also not be detected by this action, and hence the wait of a preempted action will either timeout or succeed by accident.
+
+        """
+        self.status = ACTION_STATUS_ENUM.EXECUTING
+        self.is_action_done = termination_condition
+        self._default_timeout = default_timeout
+        self._default_sleep_resolution = default_sleep_resolution
+
+    def wait(self, timeout: Optional[float] = None, sleep_resolution: Optional[float] = None) -> ACTION_STATUS_ENUM:
+        """Busy waiting until the termination condition returns true, or until timeout.
+
+        Args:
+            timeout (float, optional): The max waiting time before the wait returns and raises a warning.
+            This prevents infinite loops. Defaults to the value set during creation of the awaitable, which is usually an appropriate value for the command.
+
+            sleep_resolution (float, optional): The length of the time.sleep() in each iteration.
+            higher values will take up less CPU resources but will also cause more latency between the action finishing and
+            this method to return. Defaults to the value set during creation of the awaitable, which is usually an appropriate value for the command.
+
+            Keep in mind that the time.sleep() function has limited accuracy, so the actual sleeping time will be usually higher
+            due to scheduling activities of the OS. Take a look [here](https://github.com/airo-ugent/airo-mono/pull/21#discussion_r1132520057)
+            for some realistic numbers on the error. The error is independent of the sleep time and is in the order of 0.2ms.
+            So the lower the sleep time, the higher the relative error becomes.
+
+
+        Returns:
+            ACTION_STATUS_ENUM: _description_
+        """
+        # see #airo-robots/scripts/measure_sleep_accuracy.py for a script that measures the sleep accuracy and
+        # the result of some measurements.
+        sleep_resolution = sleep_resolution or self._default_sleep_resolution
+        timeout = timeout or self._default_timeout
+        assert (
+            sleep_resolution > 0.001
+        ), "sleep resolution must be at least 1 ms, otherwise the relative error of a sleep becomes too large to be meaningful"
+        if not self.status == ACTION_STATUS_ENUM.EXECUTING:
+            return self.status
+        while True:
+            time.sleep(sleep_resolution)
+            timeout -= sleep_resolution
+            if self.is_action_done():
+                self.status = ACTION_STATUS_ENUM.SUCCEEDED
+                return self.status
+            if timeout < 0:
+                warnings.warn("Action timed out. Make sure this was expected.")
+                return ACTION_STATUS_ENUM.TIMEOUT
+
+    def is_done(self) -> bool:
+        return self.status == ACTION_STATUS_ENUM.SUCCEEDED

airo_robots/drives/hardware/kelo_robile.py

@@ -0,0 +1,133 @@
+import math
+import time
+from threading import Thread
+
+import numpy as np
+from airo_robots.awaitable_action import AwaitableAction
+from airo_robots.drives.mobile_robot import CompliantLevel, MobileRobot
+from airo_tulip.api.client import KELORobile as KELORobileClient  # type: ignore
+from airo_tulip.hardware.platform_driver import PlatformDriverType  # type: ignore
+from airo_typing import Vector3DType
+
+
+class KELORobile(MobileRobot):
+    """KELO Robile platform implementation of the MobileRobot interface.
+
+    The KELO Robile platform consists of several drives with castor wheels which are connected via EtherCAT and can
+    be controlled individually. The airo-tulip API, which is used here, provides a higher level interface which
+    controls the entire platform."""
+
+    def __init__(self, robot_ip: str, robot_port: int = 49789):
+        """Connect to the KELO robot.
+
+        The KELO robot should already be running the airo-tulip server. If this is not the case, any messages
+        sent from the client may be queued and executed once the server is started, resulting in unexpected
+        and/or sudden movements.
+
+        Args:
+            robot_ip: IP address of the KELO CPU brick.
+            robot_port: Port to connect on (default: 49789)."""
+
+        self._kelo_robile = KELORobileClient(robot_ip, robot_port)
+
+        # Position control.
+        self._control_loop_done = True
+
+    def align_drives(self, x: float, y: float, a: float, timeout: float = 1.0) -> AwaitableAction:
+        """Align all drives for driving in a direction given by the linear and angular velocities.
+
+        Beware that sending any other velocity commands before the awaitable is done may cause unexpected behaviour,
+        as this affects the "aligned" condition.
+
+        Args:
+            x: The x velocity (linear, m/s).
+            y: The y velocity (linear, m/s)
+            a: The angular velocity (rad/s).
+            timeout: The awaitable will finish after this time at the latest.
+
+        Returns:
+            An AwaitableAction which will check if the drives are aligned, or if the timeout has expired."""
+        self._kelo_robile.align_drives(x, y, a, timeout=timeout)
+
+        action_sent_time = time.time_ns()
+        return AwaitableAction(
+            lambda: self._kelo_robile.are_drives_aligned() or time.time_ns() - action_sent_time > timeout * 1e9,
+            default_timeout=2 * timeout,
+            default_sleep_resolution=0.002,
+        )
+
+    def set_platform_velocity_target(self, x: float, y: float, a: float, timeout: float) -> AwaitableAction:
+        self._kelo_robile.set_platform_velocity_target(x, y, a, timeout=timeout)
+
+        action_sent_time = time.time_ns()
+        return AwaitableAction(
+            lambda: time.time_ns() - action_sent_time > timeout * 1e9,
+            default_timeout=2 * timeout,
+            default_sleep_resolution=0.002,
+        )
+
+    def _move_platform_to_pose_control_loop(
+        self, target_pose: Vector3DType, action_start_time: float, action_timeout_time: float, timeout: float
+    ) -> None:
+        stop = False
+        while not stop:
+            current_pose = self._kelo_robile.get_odometry()
+            delta_pose = target_pose - current_pose
+
+            vel_vec_angle = np.arctan2(delta_pose[1], delta_pose[0]) - current_pose[2]
+            vel_vec_norm = min(np.linalg.norm(delta_pose[:2]), 0.5)
+            vel_x = vel_vec_norm * np.cos(vel_vec_angle)
+            vel_y = vel_vec_norm * np.sin(vel_vec_angle)
+
+            delta_angle = np.arctan2(np.sin(delta_pose[2]), np.cos(delta_pose[2]))
+            vel_a = max(min(delta_angle, math.pi / 4), -math.pi / 4)
+
+            command_timeout = (action_timeout_time - time.time_ns()) * 1e-9
+            if command_timeout >= 0.0:
+                self._kelo_robile.set_platform_velocity_target(vel_x, vel_y, vel_a, timeout=command_timeout)
+
+            at_target_pose = bool(np.linalg.norm(delta_pose) < 0.01)
+            stop = at_target_pose or time.time_ns() - action_start_time > timeout * 1e9
+
+        self._kelo_robile.set_platform_velocity_target(0.0, 0.0, 0.0)
+        self._control_loop_done = True
+
+    def move_platform_to_pose(self, x: float, y: float, a: float, timeout: float) -> AwaitableAction:
+        target_pose = np.array([x, y, a])
+        action_start_time = time.time_ns()
+        action_timeout_time = action_start_time + timeout * 1e9
+
+        self._control_loop_done = False  # Will be set to True by the below thread once it's finished.
+        thread = Thread(
+            target=self._move_platform_to_pose_control_loop,
+            args=(target_pose, action_start_time, action_timeout_time, timeout),
+        )
+        thread.start()
+
+        return AwaitableAction(
+            lambda: self._control_loop_done,
+            default_timeout=2 * timeout,
+            default_sleep_resolution=0.002,
+        )
+
+    def enable_compliant_mode(
+        self, enabled: bool, compliant_level: CompliantLevel = CompliantLevel.COMPLIANT_WEAK
+    ) -> None:
+        if enabled:
+            if compliant_level == CompliantLevel.COMPLIANT_WEAK:
+                self._kelo_robile.set_driver_type(PlatformDriverType.COMPLIANT_WEAK)
+            elif compliant_level == CompliantLevel.COMPLIANT_MODERATE:
+                self._kelo_robile.set_driver_type(PlatformDriverType.COMPLIANT_MODERATE)
+            else:
+                self._kelo_robile.set_driver_type(PlatformDriverType.COMPLIANT_STRONG)
+        else:
+            self._kelo_robile.set_driver_type(PlatformDriverType.VELOCITY)
+
+    def get_odometry(self) -> Vector3DType:
+        return self._kelo_robile.get_odometry()
+
+    def get_velocity(self) -> Vector3DType:
+        return self._kelo_robile.get_velocity()
+
+    def reset_odometry(self) -> None:
+        self._kelo_robile.reset_odometry()

airo_robots/drives/hardware/manual_robot_testing.py

@@ -0,0 +1,31 @@
+"""code for manual testing of mobile robot base class implementations.
+"""
+from airo_robots.drives.hardware.kelo_robile import KELORobile
+from airo_robots.drives.mobile_robot import MobileRobot
+
+
+def manually_test_robot_implementation(robot: MobileRobot) -> None:
+    if isinstance(robot, KELORobile):
+        input("robot will rotate drives")
+        robot.align_drives(0.1, 0.0, 0.0, 1.0).wait()
+
+    input("robot will now move forward 10cm")
+    robot.set_platform_velocity_target(0.1, 0.0, 0.0, 1.0).wait()
+
+    input("robot will now move left 10cm")
+    robot.set_platform_velocity_target(0.0, 0.1, 0.0, 1.0).wait()
+
+    input("robot will now make two short rotations")
+    robot.set_platform_velocity_target(0.0, 0.0, 0.1, 1.0).wait()
+    robot.set_platform_velocity_target(0.0, 0.0, -0.1, 1.0).wait()
+
+    input("robot will now return to original position")
+    robot.set_platform_velocity_target(-0.1, -0.1, 0.0, 1.0).wait()
+
+    input("robot will now move to 10 cm along +X relative from its starting position with position control")
+    robot.move_platform_to_pose(0.1, 0.0, 0.0, 10.0).wait()
+
+
+if __name__ == "__main__":
+    mobi = KELORobile("10.10.129.21")
+    manually_test_robot_implementation(mobi)

airo_robots/drives/mobile_robot.py

@@ -0,0 +1,82 @@
+from abc import ABC, abstractmethod
+from enum import Enum
+
+from airo_robots.awaitable_action import AwaitableAction
+from airo_typing import Vector3DType
+
+
+class CompliantLevel(Enum):
+    """The level of compliance expected from the mobile robot.
+
+    Values may not correspond to identical behaviour on different mobile platforms, but are merely an indication.
+    A value of weak means a very compliant robot, whereas a value of strong means a slightly compliant robot."""
+
+    COMPLIANT_WEAK = 1
+    COMPLIANT_MODERATE = 2
+    COMPLIANT_STRONG = 3
+
+
+class MobileRobot(ABC):
+    """
+    Base class for a mobile robot.
+
+    Mobile robots typically allow to set a target velocity for the entire platform and apply torques to their wheels
+    to achieve the desired velocity.
+
+    All values are in metric units:
+    - Linear velocities are expressed in meters/second
+    - Angular velocities are expressed in radians/second
+    """
+
+    @abstractmethod
+    def set_platform_velocity_target(self, x: float, y: float, a: float, timeout: float) -> AwaitableAction:
+        """Set the desired platform velocity.
+
+        Args:
+            x: Linear velocity along the X axis.
+            y: Linear velocity along the Y axis.
+            a: Angular velocity.
+            timeout: After this time, the platform will automatically stop.
+
+        Returns:
+            An awaitable action."""
+
+    @abstractmethod
+    def move_platform_to_pose(self, x: float, y: float, a: float, timeout: float) -> AwaitableAction:
+        """Move the platform to the given pose, without guarantees about the followed path.
+
+        Args:
+            x: Position along the startup pose's X axis.
+            y: Position along the startup pose's Y axis.
+            a: Orientation around the startup pose's Z axis.
+            timeout: After this time, the platform will automatically stop.
+
+        Returns:
+            An awaitable action."""
+
+    @abstractmethod
+    def enable_compliant_mode(self, enabled: bool, compliant_level: CompliantLevel) -> None:
+        """Enable compliant mode on the robot.
+
+        Args:
+            enabled: If true, will enable compliant mode. Else, will disable compliant mode.
+            compliant_level: The level of compliance to be expected from the robot. Ignored if `enabled` is `False`."""
+
+    @abstractmethod
+    def get_odometry(self) -> Vector3DType:
+        """Get the estimated robot pose as a 3D vector comprising the `x`, `y`, and `theta` values relative to the
+        robot's starting pose.
+
+        Returns: A 3D vector with a value for the `x` position, `y` position, and `theta` angle of the robot."""
+
+    @abstractmethod
+    def reset_odometry(self) -> None:
+        """Reset the robot's odometry to `(0, 0, 0)`."""
+
+    @abstractmethod
+    def get_velocity(self) -> Vector3DType:
+        """Get the estimated robot's velocity as a 3D vector comprising the `x`, `y` and `theta` values relative to the
+        robot's starting pose.
+
+        Returns:
+            A 3D vector with a value for the `x` velocity, `y` velocity, and `theta` angular velocity of the robot."""

airo_robots/grippers/__init__.py

@@ -0,0 +1,5 @@
+from airo_robots.grippers.hardware.robotiq_2f85_urcap import Robotiq2F85  # noqa: F401 - imported but unused
+from airo_robots.grippers.parallel_position_gripper import (  # noqa: F401 - imported but unused
+    ParallelPositionGripper,
+    ParallelPositionGripperSpecs,
+)

airo_robots/grippers/hardware/manual_gripper_testing.py

@@ -0,0 +1,32 @@
+"""code for manual testing of gripper base class implementations.
+"""
+from airo_robots.grippers.parallel_position_gripper import ParallelPositionGripper, ParallelPositionGripperSpecs
+
+
+def manually_test_gripper_implementation(
+    gripper: ParallelPositionGripper, specs: ParallelPositionGripperSpecs
+) -> None:
+    input("gripper will now open")
+    gripper.move(specs.max_width).wait()
+    assert abs(gripper.get_current_width() - specs.max_width) < 0.003
+    print(f"gripper width {gripper.get_current_width()} should be close to the requested {specs.max_width}")
+
+    input("set speed and check that this happens synchronously")
+    gripper.speed = 0.02
+    print(f"{gripper.speed=}")
+    assert abs(gripper.speed - 0.02) < 0.005
+
+    input("gripper wil now move slowly to 2cm opening ")
+    res = gripper.move(0.02)
+    print("move started and returned awaitable")
+    res.wait()
+    print("move completed, gripper should have reached 2cm")
+    assert abs(gripper.get_current_width() - 0.02) < 0.003
+
+    input("reopen gripper fast")
+    gripper.move(0.08, 0.15)
+    input("close with high force, you can put an object between the fingers to test the force and the grasp detection")
+    gripper.max_grasp_force = 200
+    gripper.speed = 0.02
+    gripper.move(0.02).wait()
+    print(f"{gripper.is_an_object_grasped()=}")

airo_robots/grippers/hardware/robotiq_2f85_urcap.py

@@ -0,0 +1,253 @@
+import asyncio
+import socket
+import time
+from typing import Optional
+
+import numpy as np
+from airo_robots.awaitable_action import AwaitableAction
+from airo_robots.grippers.hardware.manual_gripper_testing import manually_test_gripper_implementation
+from airo_robots.grippers.parallel_position_gripper import ParallelPositionGripper, ParallelPositionGripperSpecs
+from airo_robots.hardware_interaction_utils import wait_for_condition_with_timeout
+
+
+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 Robotiq2F85(ParallelPositionGripper):
+    """
+    Implementation of the gripper interface for a Robotiq 2F-85 gripper that is connected to a UR robot and is controlled with the Robotiq URCap.
+
+    The API is available at TCP port 63352 of the UR controller and wraps the Modbus registers of the gripper, as described in
+    https://dof.robotiq.com/discussion/2420/control-robotiq-gripper-mounted-on-ur-robot-via-socket-communication-python.
+    The control sequence is gripper motor <---- gripper registers<--ModbusSerial(rs485)-- UR controller <--TCP-- remote control
+
+    This class does 2 things:
+    - it communicates over TCP using the above mentioned API to read/write values from/to the gripper's registers.
+    - it rescales all those register values into metric units, as required by the gripper interface
+
+    For more info on how to install the URCap and connecting the gripper's RS-485 connection to a UR robot, see the manual, section 4.8
+    https://assets.robotiq.com/website-assets/support_documents/document/2F-85_2F-140_Instruction_Manual_e-Series_PDF_20190206.pdf
+
+    Make sure you can control the gripper using the robot controller (polyscope) before using this script.
+    """
+
+    # values obtained from the manual
+    # see https://assets.robotiq.com/website-assets/support_documents/document/2F-85_2F-140_Instruction_Manual_e-Series_PDF_20190206.pdf
+    ROBOTIQ_2F85_DEFAULT_SPECS = ParallelPositionGripperSpecs(0.085, 0.0, 220, 25, 0.15, 0.02)
+
+    def __init__(self, host_ip: str, port: int = 63352, fingers_max_stroke: Optional[float] = None) -> None:
+        """
+        host_ip: the IP adress of the robot to which the gripper is connected.
+
+        fingers_max_stroke:
+        allow for custom max stroke width (if you have fingertips that are closer together).
+        the robotiq will always calibrate such that max opening = 0 and min opening = 255 for its register.
+        see manual p42
+        """
+        self._gripper_specs = self.ROBOTIQ_2F85_DEFAULT_SPECS
+        if fingers_max_stroke:
+            self._gripper_specs.max_width = fingers_max_stroke
+        self.host_ip = host_ip
+        self.port = port
+
+        self._check_connection()
+
+        if not self.gripper_is_active():
+            self._activate_gripper()
+
+        super().__init__(self.gripper_specs)
+
+    def get_current_width(self) -> float:
+        register_value = int(self._communicate("GET POS").split(" ")[1])
+        width = rescale_range(register_value, 0, 230, self._gripper_specs.max_width, self._gripper_specs.min_width)
+        return width
+
+    def move(self, width: float, speed: Optional[float] = None, force: Optional[float] = None) -> AwaitableAction:
+        if speed:
+            self.speed = speed
+        if force:
+            self.max_grasp_force = force
+        self._set_target_width(width)
+
+        # this sleep is required to make sure that the OBJ STATUS
+        # of the gripper is already in 'moving' before entering the wait loop.
+        time.sleep(0.01)
+
+        def move_done_condition() -> bool:
+            done = abs(self.get_current_width() - width) < 0.002
+            done = done or self.is_an_object_grasped()
+            return done
+
+        return AwaitableAction(move_done_condition)
+
+    def is_an_object_grasped(self) -> bool:
+        return int(self._communicate("GET OBJ").split(" ")[1]) == 2
+
+    @property
+    def speed(self) -> float:
+        speed_register_value = self._read_speed_register()
+        return rescale_range(
+            speed_register_value, 0, 255, self._gripper_specs.min_speed, self._gripper_specs.max_speed
+        )
+
+    @speed.setter
+    def speed(self, value: float) -> None:
+        speed = np.clip(value, self.gripper_specs.min_speed, self.gripper_specs.max_speed)
+        speed_register_value = int(
+            rescale_range(speed, self._gripper_specs.min_speed, self._gripper_specs.max_speed, 0, 255)
+        )
+        self._communicate(f"SET SPE {speed_register_value}")
+
+        def is_value_set() -> bool:
+            return self._is_target_value_set(self._read_speed_register(), speed_register_value)
+
+        wait_for_condition_with_timeout(is_value_set)
+
+    @property
+    def max_grasp_force(self) -> float:
+        force_register_value = self._read_force_register()
+        # 0 force has a special meaning, cf manual.
+        return rescale_range(
+            force_register_value, 1, 255, self._gripper_specs.min_force, self._gripper_specs.max_force
+        )
+
+    @max_grasp_force.setter
+    def max_grasp_force(self, value: float) -> None:
+        force = np.clip(value, self.gripper_specs.min_force, self.gripper_specs.max_force)
+        force_register_value = int(
+            rescale_range(force, self._gripper_specs.min_force, self._gripper_specs.max_force, 1, 255)
+        )
+        self._communicate(f"SET FOR {force_register_value}")
+
+        def is_value_set() -> bool:
+            return self._is_target_value_set(force_register_value, self._read_force_register())
+
+        wait_for_condition_with_timeout(is_value_set)
+
+    ###########################
+    ## non-interface classes ##
+    ###########################
+    async def asyncio_move(self, width: float, speed: Optional[float] = None, force: Optional[float] = None) -> None:
+        """Asyncio (async) move"""
+        if speed:
+            self.speed = speed
+        if force:
+            self.max_grasp_force = force
+        self._set_target_width(width)
+
+        while self.is_gripper_moving():
+            await asyncio.sleep(0.05)
+
+    ####################
+    # Private methods #
+    ####################
+
+    def _set_target_width(self, target_width_in_meters: float) -> None:
+        """Sends target width to gripper"""
+        target_width_in_meters = np.clip(
+            target_width_in_meters, self._gripper_specs.min_width, self._gripper_specs.max_width
+        )
+        # 230 is 'force closed', cf _write_target_width_to_register.
+        target_width_register_value = round(
+            rescale_range(target_width_in_meters, self._gripper_specs.min_width, self._gripper_specs.max_width, 230, 0)
+        )
+        self._write_target_width_to_register(target_width_register_value)
+
+    def _write_target_width_to_register(self, target_width_register_value: int) -> None:
+        """
+        Takes values in range 0 -255
+        3 is actually fully open, 230 is fully closed in operating mode (straight fingers) and 255 is fully closed in encompassed mode.
+        For the default fingertips, the range 3 - 230 maps approximately to 0mm - 85mm with a quasi linear relation of 0.4mm / unit
+        (experimental findings, but manual also mentions this 0.4mm/unit relation)
+
+        For custom finger tips with a different stroke, it won't be 0.4mm/unit anymore,
+        but 230 will still be fully closed due to the self-calibration of the gripper.
+
+        """
+        self._communicate(f"SET  POS {target_width_register_value}")
+
+        def is_value_set() -> bool:
+            return self._is_target_value_set(target_width_register_value, self._read_target_width_register())
+
+        wait_for_condition_with_timeout(is_value_set)
+
+    def _read_target_width_register(self) -> int:
+        return int(self._communicate("GET PRE").split(" ")[1])
+
+    def _read_speed_register(self) -> int:
+        return int(self._communicate("GET SPE").split(" ")[1])
+
+    def _read_force_register(self) -> int:
+        return int(self._communicate("GET FOR").split(" ")[1])
+
+    def is_gripper_moving(self) -> bool:
+        # Moving == 0 => detected OR position reached
+        return int(self._communicate("GET OBJ").split(" ")[1]) == 0
+
+    def _check_connection(self) -> None:
+        """validate communication with gripper is possible.
+        Raises:
+            ConnectionError
+        """
+        if not self._communicate("GET STA").startswith("STA"):
+            raise ConnectionError("Could not connect to gripper")
+
+    def _communicate(self, command: str) -> str:
+        """Helper function to communicate with gripper over a tcp socket.
+        Args:
+            command (str): The GET/SET command string.
+        """
+        with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
+            try:
+                s.connect((self.host_ip, self.port))
+                s.sendall(("" + str.strip(command) + "\n").encode())
+
+                data = s.recv(2**10)
+                return data.decode()[:-1]
+            except Exception as e:
+                raise (e)
+
+    def _activate_gripper(self) -> None:
+        """Activates the gripper, sets target position to "Open" and sets GTO flag."""
+        self._communicate("SET ACT 1")
+        wait_for_condition_with_timeout(self.gripper_is_active)
+        # initialize gripper
+        self._communicate("SET GTO 1")  # enable Gripper
+        self.speed = self._gripper_specs.max_speed
+        self.force = self._gripper_specs.min_force
+
+    def _deactivate_gripper(self) -> None:
+        self._communicate("SET ACT 0")
+        wait_for_condition_with_timeout(lambda: self._communicate("GET STA") == "STA 0")
+
+    def gripper_is_active(self) -> bool:
+        return self._communicate("GET STA") == "STA 3"
+
+    @staticmethod
+    def _is_target_value_set(target: int, value: int) -> bool:
+        """helper to compare target value to current value and make the force / speed request synchronous"""
+        return abs(target - value) < 5
+
+
+def get_empirical_data_on_opening_angles(robot_ip: str) -> None:
+    """used to verify the relation between finger width and register values."""
+    gripper = Robotiq2F85(robot_ip)
+    gripper._write_target_width_to_register(230)
+    input("press")
+    for position in [0, 50, 100, 150, 200, 250]:
+        gripper._write_target_width_to_register(position)
+        input(f"press key for moving to next position {position}")
+
+
+if __name__ == "__main__":
+    robot_ip = "10.42.0.162"  # hardcoded IP of Victor UR3e
+    import click
+
+    @click.command()
+    @click.option("--robot_ip", default=robot_ip, help="IP of UR to which the gripper is connected.")
+    def test_robotiq(robot_ip: str) -> None:
+        gripper = Robotiq2F85(robot_ip)
+        manually_test_gripper_implementation(gripper, gripper._gripper_specs)
+
+    test_robotiq()