airo-robots 2025.4.0__tar.gz

airo_robots-2025.4.0/PKG-INFO

@@ -0,0 +1,16 @@
+Metadata-Version: 2.4
+Name: airo_robots
+Version: 2025.4.0
+Summary: Interfaces, hardware implementations of those interfaces and other functionalities to control robot manipulators and grippers at the Ghent University AI and Robotics Lab
+Author: Thomas Lips
+Author-email: thomas.lips@ugent.be
+Requires-Dist: numpy<2.0
+Requires-Dist: ur-rtde>=1.5.7
+Requires-Dist: click
+Requires-Dist: airo-typing==2025.4.0
+Requires-Dist: airo-spatial-algebra==2025.4.0
+Requires-Dist: airo-tulip==0.2.2
+Dynamic: author
+Dynamic: author-email
+Dynamic: requires-dist
+Dynamic: summary

airo_robots-2025.4.0/README.md

@@ -0,0 +1,87 @@
+# Airo-robots
+This package contains code for controlling robot manipulators and grippers, specifically:
+- base classes (interfaces) for the different hardware types (grippers, manipulators, F/T sensors), i.e. defines the 'driver' interfaces.
+- implementations of those interfaces for some of the hardware used at AIRO.
+- code to manually test these implementations, when the hardware is attached to the computer.
+
+The following combinations of hardware and communication options are currently implemented:
+| Hardware | Communication | Implementation |
+|----------|:----------|----------------|
+| UR robots | RTDE | [ur_rtde.py](airo_robots/manipulators/hardware/ur_rtde.py) |
+| Robotiq 2F85 gripper | URCap web API | [robotiq_2f85_urcap.py](airo_robots/grippers/hardware/robotiq_2f85_urcap.py) |
+
+Each hardware implementation module will have a `__main__` codeblock that runs the tests for that hardware implementation. This is useful to check if the hardware is connected correctly and the implementation is working as expected. But it is also the place to be to get an idea of how to use the implementation.
+
+## Async interactions
+Certain hardware commands can require some time before completion. Think about sending a move command to a robot, the exact time will depend on the velocity (profile) and the distance between the current pose and target pose, but it can easily be multiple seconds. You might nog want to be busy waiting on that command to complete before the method returns. For this reason, certain commands will send the command to the hardware, formulate a condition that signals when the command has finished and then return an [`AwaitableAction`](airo_robots/awaitable_action.py). You can then decide when (and if) you want to start busy waiting on the command to finish (according to the specified condition). This allows you to do some other useful things in the meantime.
+
+When you decide to wait you have to specify the max waiting time (the timeout value) and the granularity of the waiting. Make sure to take a look in the code so that you understand these values and can use appropriate values, especially have a look at the part about the `time.sleep` accuracy. An example interaction might look like this:
+
+```python
+action = robot.move(target_pose)
+# do other stuff, such as computing the next action
+new_action = compute_new_action()
+action.wait(timeout=2,resolution=0.2)
+# send new action
+robot.do_new_action(new_action)
+```
+
+**This is only guaranteed to work when you explicitly wait on a previous command before sending a new command**. If you send multiple commands, it is up to the robot controller to decide in what order the commands are executed (if they are not preempted). At that point, the termination condition in the `AwaitableAction` will probably make no sense anymore and result in a timeout. So make sure you know what you are doing if you send multiple actions and don't use the `wait()` method on them unless you know very well what you are doing.
+
+See [below](#notes-on-the-different-types-of-interfaces-for-hardware-interaction) for a lengthier discussion of (async) hardware interactions and why the interface is implemented like this.
+## Installation
+You can simply pip install this package. Note that some hardware implementations have additional dependencies, which are for now included in the setup.py but might be separated later on.
+## Structure
+a more detailled overview of the structure and content of this package:
+
+```
+airo_robots/
+    manipulators/
+        position_manipulator.py             # base classes for position-controlled manipulators
+        force_torque_sensor.py              # base class for FT sensors
+        bimanual_position_manipulator.py    # base class for bimanual manipulators
+        hardware/                           # contains the implementations of the inferfaces
+            manual_gripper_testing.py       # code for manually testing hw implementations
+            ur_rtde.py                      # implementation of the interfaces for UR robots over the RTDE interface
+
+    grippers/
+        parallel_position_gripper.py        # base classes for parallel-finger, position-controlled grippers
+        hardware/
+            manual_gripper_testing.py           # code for manually testing hw implementations
+            robotiq_2f85_urcap.py                 # implementations for robotiq_2F85 gripper over the URscript TCP API
+```
+
+## Adding new hardware
+
+### Adding new hardware implementations
+If an interface already exists for the hardware you want to use, you can simply use that interface and implement the hardware-specific code in a new module under the appriate `hardware/` folder. For the async methods, use an appropriate condition in the Awaitable Action object to signal when the command has finished.
+
+Don't forget to add a `__main__` codeblock to the new module that runs the tests for that hardware implementation.
+Also don't forget to add the implementation in the table above and import it in the `__init__.py` of the appropriate submodule.
+
+
+### Adding new interfaces
+Best to look at the existing interfaces for inspiration. The main thing to keep in mind is that the interface should be as general as possible, while still being useful. So don't add too many methods that are specific to a single hardware implementation. Methods for which it makes sense should return an Awaitable Action object.
+
+Dont forget to add the interface in the `__init__.py` of the appropriate submodule.
+## Notes on the different types of interfaces for Hardware interaction
+
+There are basically three ways to send commands to hardware:
+- send a command and never look back (asynchronous)
+- send a command, continue and later on check if it was succesful (asynchronous + awaitable)
+- send a command and wait for it to finish (synchronous)
+
+In ROS these are respectively sending to a topic, sending to an action server and sending + waiting on the action server.
+
+A first thing to note is that not all commands have a clear 'termination condition'. E.g. if you send a new waypoint to an admittance controller, when has has the controller succesfully handled your request? There is no clear 'end' as with a MoveL signal that has a target pose and really has to get there.
+So for these commands only the first option is available.
+
+Another complication with async commands is about what should happen when you send multiple commands in a row. Should the robot execute them in that order, or should the robot simply take the most recent command and ignore the previous ones? This is a design choice that should be made by the user of the interface. ROS (and most hardware controllers) take the last option, i.e. the robot will execute the most recent command and ignore the previous ones. These would be marked as *preempted* in ROS.
+
+To implement the awaitable behavior in python, there are a number of options:
+
+1) use AsyncIO. This might actually be the most pythonic way, but it requires all downstream code to also be async. This is not always possible, e.g. when using this code in ROS. Even when it is possible, it is not desirable imo.
+
+2) Have a custom return object on which you can wait actively when desired. This is the approach we have taken in this package.
+
+To signal preemption of commands to the user, you have to manually queue all commands and mark all other commands as preempted when popping the most recent command from the queue. This in turn requires a separate controll process (otherwise your single thread (mind the GIL), would spend lots of times checking if the current command is finished, even if you don't care about that at all). We considered this complexity unjustified and hence do not detect/enforce/control preemptions. It is up to the user to either avoid sending multiple commands or to check how the hardware controllers deals with them. Note that waiting on preempted or stale commands will result in a timeout.

airo_robots-2025.4.0/airo_robots/__init__.py

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

airo_robots-2025.4.0/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-2025.4.0/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-2025.4.0/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-2025.4.0/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-2025.4.0/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-2025.4.0/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()=}")