From 5e39defe7ef2765fc3de4b89216920f10b7d5400 Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mathieu.decoster@ugent.be>
Date: Mon, 15 Jul 2024 09:40:00 +0200
Subject: [PATCH 01/30] First implementation of KELORobile for airo-tulip
 v0.0.1

---
 airo-robots/airo_robots/drives/__init__.py    |  0
 .../airo_robots/drives/hardware/__init__.py   |  0
 .../drives/hardware/kelo_robile.py            | 32 +++++++++++++++++++
 .../drives/hardware/manual_robot_testing.py   | 18 +++++++++++
 .../airo_robots/drives/mobile_robot.py        | 29 +++++++++++++++++
 airo-robots/setup.py                          |  1 +
 6 files changed, 80 insertions(+)
 create mode 100644 airo-robots/airo_robots/drives/__init__.py
 create mode 100644 airo-robots/airo_robots/drives/hardware/__init__.py
 create mode 100644 airo-robots/airo_robots/drives/hardware/kelo_robile.py
 create mode 100644 airo-robots/airo_robots/drives/hardware/manual_robot_testing.py
 create mode 100644 airo-robots/airo_robots/drives/mobile_robot.py

diff --git a/airo-robots/airo_robots/drives/__init__.py b/airo-robots/airo_robots/drives/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/airo-robots/airo_robots/drives/hardware/__init__.py b/airo-robots/airo_robots/drives/hardware/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile.py b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
new file mode 100644
index 00000000..1eea39d4
--- /dev/null
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
@@ -0,0 +1,32 @@
+import time
+
+from airo_robots.awaitable_action import AwaitableAction
+from airo_robots.drives.mobile_robot import MobileRobot
+from airo_tulip.server.kelo_robile import KELORobile as KELORobileClient
+
+
+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):
+        """Connect to the KELO robot.
+
+        The KELO robot should already be running the airo-tulip server.
+
+        Args:
+            robot_ip: IP address of the KELO CPU brick.
+            robot_port: Port to connect on."""
+        self._kelo_robile = KELORobileClient(robot_ip, robot_port)
+
+    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)
+
+        def timeout_awaitable() -> bool:
+            time.sleep(timeout)
+            return True
+
+        return AwaitableAction(timeout_awaitable)
diff --git a/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py b/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py
new file mode 100644
index 00000000..eabade3c
--- /dev/null
+++ b/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py
@@ -0,0 +1,18 @@
+"""code for manual testing of mobile robot base class implementations.
+"""
+from airo_robots.drives.mobile_robot import MobileRobot
+
+
+def manually_test_robot_implementation(robot: MobileRobot) -> None:
+    input("robot will now move forward 10cm")
+    robot.set_platform_velocity_target(0.1, 0.0, 0.0, 1.0)
+
+    input("robot will now move left 10cm")
+    robot.set_platform_velocity_target(0.0, 0.1, 0.0, 1.0)
+
+    input("robot will now make two short rotations")
+    robot.set_platform_velocity_target(0.0, 0.0, 0.1, 1.0)
+    robot.set_platform_velocity_target(0.0, 0.0, -0.1, 1.0)
+
+    input("robot will now return to original position")
+    robot.set_platform_velocity_target(-0.1, -0.1, 0.0, 1.0)
diff --git a/airo-robots/airo_robots/drives/mobile_robot.py b/airo-robots/airo_robots/drives/mobile_robot.py
new file mode 100644
index 00000000..5f4d86fd
--- /dev/null
+++ b/airo-robots/airo_robots/drives/mobile_robot.py
@@ -0,0 +1,29 @@
+from abc import ABC, abstractmethod
+
+from airo_robots.awaitable_action import AwaitableAction
+
+
+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."""
diff --git a/airo-robots/setup.py b/airo-robots/setup.py
index 5ac04e77..f00d121c 100644
--- a/airo-robots/setup.py
+++ b/airo-robots/setup.py
@@ -15,6 +15,7 @@
         "click",
         "airo-typing",
         "airo-spatial-algebra",
+        "airo-tulip==0.0.1a0",
     ],
     packages=setuptools.find_packages(),
     package_data={"airo_robots": ["py.typed"]},

From 6dcbbcd142f222191704a8512d176f45cc21347d Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mathieu.decoster@ugent.be>
Date: Mon, 15 Jul 2024 09:41:20 +0200
Subject: [PATCH 02/30] Add wait() to robot testing

---
 .../drives/hardware/manual_robot_testing.py            | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py b/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py
index eabade3c..878dd25a 100644
--- a/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py
+++ b/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py
@@ -5,14 +5,14 @@
 
 def manually_test_robot_implementation(robot: MobileRobot) -> None:
     input("robot will now move forward 10cm")
-    robot.set_platform_velocity_target(0.1, 0.0, 0.0, 1.0)
+    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)
+    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)
-    robot.set_platform_velocity_target(0.0, 0.0, -0.1, 1.0)
+    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)
+    robot.set_platform_velocity_target(-0.1, -0.1, 0.0, 1.0).wait()

From 952999e1a3633105dfbe04e4440dbeb2fa440bb8 Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Wed, 14 Aug 2024 11:16:24 +0200
Subject: [PATCH 03/30] Update airo-tulip to 0.0.3 for compliant mode

---
 .../drives/hardware/kelo_robile.py            | 19 ++++++++++++++++---
 .../airo_robots/drives/mobile_robot.py        | 11 ++++++++++-
 airo-robots/setup.py                          |  2 +-
 3 files changed, 27 insertions(+), 5 deletions(-)

diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile.py b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
index 1eea39d4..99df253b 100644
--- a/airo-robots/airo_robots/drives/hardware/kelo_robile.py
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
@@ -1,8 +1,10 @@
 import time
 
+from airo_tulip.platform_driver import PlatformDriverType
+from airo_tulip.server.kelo_robile import KELORobile as KELORobileClient
+
 from airo_robots.awaitable_action import AwaitableAction
 from airo_robots.drives.mobile_robot import MobileRobot
-from airo_tulip.server.kelo_robile import KELORobile as KELORobileClient
 
 
 class KELORobile(MobileRobot):
@@ -22,11 +24,22 @@ def __init__(self, robot_ip: str, robot_port: int):
             robot_port: Port to connect on."""
         self._kelo_robile = KELORobileClient(robot_ip, robot_port)
 
-    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)
+    def set_platform_velocity_target(self, x: float, y: float, a: float, timeout: float,
+                                     align_drives_first: bool = False) -> AwaitableAction:
+        if align_drives_first:
+            self._kelo_robile.set_platform_velocity_target(x, y, a, timeout=1.0, instantaneous=False,
+                                                           only_align_drives=True)
+
+        self._kelo_robile.set_platform_velocity_target(x, y, a, timeout=timeout, instantaneous=True)
 
         def timeout_awaitable() -> bool:
             time.sleep(timeout)
             return True
 
         return AwaitableAction(timeout_awaitable)
+
+    def enable_compliant_mode(self, enabled: bool):
+        if enabled:
+            self._kelo_robile.set_driver_type(PlatformDriverType.COMPLIANT)
+        else:
+            self._kelo_robile.set_driver_type(PlatformDriverType.VELOCITY)
diff --git a/airo-robots/airo_robots/drives/mobile_robot.py b/airo-robots/airo_robots/drives/mobile_robot.py
index 5f4d86fd..3976866d 100644
--- a/airo-robots/airo_robots/drives/mobile_robot.py
+++ b/airo-robots/airo_robots/drives/mobile_robot.py
@@ -16,7 +16,8 @@ class MobileRobot(ABC):
     """
 
     @abstractmethod
-    def set_platform_velocity_target(self, x: float, y: float, a: float, timeout: float) -> AwaitableAction:
+    def set_platform_velocity_target(self, x: float, y: float, a: float, timeout: float,
+                                     align_drives_first: bool = False) -> AwaitableAction:
         """Set the desired platform velocity.
 
         Args:
@@ -24,6 +25,14 @@ def set_platform_velocity_target(self, x: float, y: float, a: float, timeout: fl
             y: Linear velocity along the Y axis.
             a: Angular velocity.
             timeout: After this time, the platform will automatically stop.
+            align_drives_first: Align drives before moving (default: False, will start driving instantly).
 
         Returns:
             An awaitable action."""
+
+    @abstractmethod
+    def enable_compliant_mode(self, enabled: bool):
+        """Enable compliant mode on the robot.
+
+        Args:
+            enabled: If true, will enable compliant mode. Else, will disable compliant mode."""
diff --git a/airo-robots/setup.py b/airo-robots/setup.py
index f00d121c..feaa6315 100644
--- a/airo-robots/setup.py
+++ b/airo-robots/setup.py
@@ -15,7 +15,7 @@
         "click",
         "airo-typing",
         "airo-spatial-algebra",
-        "airo-tulip==0.0.1a0",
+        "airo-tulip==0.0.3",
     ],
     packages=setuptools.find_packages(),
     package_data={"airo_robots": ["py.typed"]},

From a131a7141247a9cea4a30e2eff01d8f2263a810c Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Tue, 20 Aug 2024 15:05:06 +0200
Subject: [PATCH 04/30] Compliant modes

---
 airo-robots/airo_robots/drives/hardware/kelo_robile.py | 9 +++++++--
 airo-robots/setup.py                                   | 2 +-
 2 files changed, 8 insertions(+), 3 deletions(-)

diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile.py b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
index 99df253b..e7a432f2 100644
--- a/airo-robots/airo_robots/drives/hardware/kelo_robile.py
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
@@ -38,8 +38,13 @@ def timeout_awaitable() -> bool:
 
         return AwaitableAction(timeout_awaitable)
 
-    def enable_compliant_mode(self, enabled: bool):
+    def enable_compliant_mode(self, enabled: bool, compliant_level: int = 1):  # TODO: Use enum variants, document.
         if enabled:
-            self._kelo_robile.set_driver_type(PlatformDriverType.COMPLIANT)
+            if compliant_level == 1:
+                self._kelo_robile.set_driver_type(PlatformDriverType.COMPLIANT_WEAK)
+            elif compliant_level == 2:
+                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)
diff --git a/airo-robots/setup.py b/airo-robots/setup.py
index feaa6315..727b8e6c 100644
--- a/airo-robots/setup.py
+++ b/airo-robots/setup.py
@@ -15,7 +15,7 @@
         "click",
         "airo-typing",
         "airo-spatial-algebra",
-        "airo-tulip==0.0.3",
+        "airo-tulip==0.0.4",
     ],
     packages=setuptools.find_packages(),
     package_data={"airo_robots": ["py.typed"]},

From bb00fa10b2346b91aa48d752f9f92a339492b923 Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Thu, 22 Aug 2024 15:26:42 +0200
Subject: [PATCH 05/30] Update airo-tulip to 0.0.5 (align drives)

---
 .../drives/hardware/kelo_robile.py            | 27 +++++++++++++++----
 .../drives/hardware/manual_robot_testing.py   |  5 ++++
 airo-robots/setup.py                          |  2 +-
 3 files changed, 28 insertions(+), 6 deletions(-)

diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile.py b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
index e7a432f2..646bcf76 100644
--- a/airo-robots/airo_robots/drives/hardware/kelo_robile.py
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
@@ -1,4 +1,5 @@
 import time
+from functools import partial
 
 from airo_tulip.platform_driver import PlatformDriverType
 from airo_tulip.server.kelo_robile import KELORobile as KELORobileClient
@@ -24,12 +25,28 @@ def __init__(self, robot_ip: str, robot_port: int):
             robot_port: Port to connect on."""
         self._kelo_robile = KELORobileClient(robot_ip, robot_port)
 
-    def set_platform_velocity_target(self, x: float, y: float, a: float, timeout: float,
-                                     align_drives_first: bool = False) -> AwaitableAction:
-        if align_drives_first:
-            self._kelo_robile.set_platform_velocity_target(x, y, a, timeout=1.0, instantaneous=False,
-                                                           only_align_drives=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)
+
+        def aligned_awaitable(start_time: float) -> bool:
+            return self._kelo_robile.are_drives_aligned() or time.time() > start_time + timeout
+
+        return AwaitableAction(partial(aligned_awaitable, time.time()))
+
+    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, instantaneous=True)
 
         def timeout_awaitable() -> bool:
diff --git a/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py b/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py
index 878dd25a..b74a50f7 100644
--- a/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py
+++ b/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py
@@ -1,9 +1,14 @@
 """code for manual testing of mobile robot base class implementations.
 """
 from airo_robots.drives.mobile_robot import MobileRobot
+from airo_robots.drives.hardware.kelo_robile import KELORobile
 
 
 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()
 
diff --git a/airo-robots/setup.py b/airo-robots/setup.py
index 727b8e6c..be838f83 100644
--- a/airo-robots/setup.py
+++ b/airo-robots/setup.py
@@ -15,7 +15,7 @@
         "click",
         "airo-typing",
         "airo-spatial-algebra",
-        "airo-tulip==0.0.4",
+        "airo-tulip==0.0.5",
     ],
     packages=setuptools.find_packages(),
     package_data={"airo_robots": ["py.typed"]},

From b012b19baa80cd6b53bcfd42150a504a013b293e Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Thu, 22 Aug 2024 15:49:50 +0200
Subject: [PATCH 06/30] Remove instantaneous argument that is no longer
 supported

---
 airo-robots/airo_robots/drives/hardware/kelo_robile.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile.py b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
index 646bcf76..b2a962c5 100644
--- a/airo-robots/airo_robots/drives/hardware/kelo_robile.py
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
@@ -47,7 +47,7 @@ def aligned_awaitable(start_time: float) -> bool:
         return AwaitableAction(partial(aligned_awaitable, time.time()))
 
     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, instantaneous=True)
+        self._kelo_robile.set_platform_velocity_target(x, y, a, timeout=timeout)
 
         def timeout_awaitable() -> bool:
             time.sleep(timeout)

From 0d5442321299a926b73d4a8d28871300c1d075f0 Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Thu, 22 Aug 2024 15:51:16 +0200
Subject: [PATCH 07/30] airo-tulip -> 0.0.6

---
 airo-robots/setup.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/airo-robots/setup.py b/airo-robots/setup.py
index be838f83..492a2986 100644
--- a/airo-robots/setup.py
+++ b/airo-robots/setup.py
@@ -15,7 +15,7 @@
         "click",
         "airo-typing",
         "airo-spatial-algebra",
-        "airo-tulip==0.0.5",
+        "airo-tulip==0.0.6",
     ],
     packages=setuptools.find_packages(),
     package_data={"airo_robots": ["py.typed"]},

From 4d7beaf2be14e4edfcf2101a0355413fa5136d65 Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Thu, 22 Aug 2024 17:43:15 +0200
Subject: [PATCH 08/30] Use Enums for MobileRobot compliance levels instead of
 integers

---
 .../airo_robots/drives/hardware/kelo_robile.py    |  8 ++++----
 airo-robots/airo_robots/drives/mobile_robot.py    | 15 +++++++++++++--
 2 files changed, 17 insertions(+), 6 deletions(-)

diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile.py b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
index b2a962c5..2f10aa60 100644
--- a/airo-robots/airo_robots/drives/hardware/kelo_robile.py
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
@@ -5,7 +5,7 @@
 from airo_tulip.server.kelo_robile import KELORobile as KELORobileClient
 
 from airo_robots.awaitable_action import AwaitableAction
-from airo_robots.drives.mobile_robot import MobileRobot
+from airo_robots.drives.mobile_robot import MobileRobot, CompliantLevel
 
 
 class KELORobile(MobileRobot):
@@ -55,11 +55,11 @@ def timeout_awaitable() -> bool:
 
         return AwaitableAction(timeout_awaitable)
 
-    def enable_compliant_mode(self, enabled: bool, compliant_level: int = 1):  # TODO: Use enum variants, document.
+    def enable_compliant_mode(self, enabled: bool, compliant_level: CompliantLevel = CompliantLevel.COMPLIANT_WEAK):
         if enabled:
-            if compliant_level == 1:
+            if compliant_level == CompliantLevel.COMPLIANT_WEAK:
                 self._kelo_robile.set_driver_type(PlatformDriverType.COMPLIANT_WEAK)
-            elif compliant_level == 2:
+            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)
diff --git a/airo-robots/airo_robots/drives/mobile_robot.py b/airo-robots/airo_robots/drives/mobile_robot.py
index 3976866d..42ce3e0b 100644
--- a/airo-robots/airo_robots/drives/mobile_robot.py
+++ b/airo-robots/airo_robots/drives/mobile_robot.py
@@ -1,7 +1,17 @@
 from abc import ABC, abstractmethod
+from enum import Enum
 
 from airo_robots.awaitable_action import AwaitableAction
 
+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):
     """
@@ -31,8 +41,9 @@ def set_platform_velocity_target(self, x: float, y: float, a: float, timeout: fl
             An awaitable action."""
 
     @abstractmethod
-    def enable_compliant_mode(self, enabled: bool):
+    def enable_compliant_mode(self, enabled: bool, compliant_level: CompliantLevel):
         """Enable compliant mode on the robot.
 
         Args:
-            enabled: If true, will enable compliant mode. Else, will disable compliant mode."""
+            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`."""

From 538e64a190e4473481f7c1555fd043bc608a29de Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Mon, 26 Aug 2024 10:54:13 +0200
Subject: [PATCH 09/30] Add get_odometry method for Robile

---
 .../airo_robots/drives/hardware/kelo_robile.py       |  9 ++++++---
 airo-robots/airo_robots/drives/mobile_robot.py       | 12 +++++++++---
 2 files changed, 15 insertions(+), 6 deletions(-)

diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile.py b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
index 2f10aa60..aa003ea9 100644
--- a/airo-robots/airo_robots/drives/hardware/kelo_robile.py
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
@@ -1,11 +1,11 @@
 import time
 from functools import partial
 
-from airo_tulip.platform_driver import PlatformDriverType
-from airo_tulip.server.kelo_robile import KELORobile as KELORobileClient
-
 from airo_robots.awaitable_action import AwaitableAction
 from airo_robots.drives.mobile_robot import MobileRobot, CompliantLevel
+from airo_tulip.platform_driver import PlatformDriverType
+from airo_tulip.server.kelo_robile import KELORobile as KELORobileClient
+from airo_typing import Vector3DType
 
 
 class KELORobile(MobileRobot):
@@ -65,3 +65,6 @@ def enable_compliant_mode(self, enabled: bool, compliant_level: CompliantLevel =
                 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()
diff --git a/airo-robots/airo_robots/drives/mobile_robot.py b/airo-robots/airo_robots/drives/mobile_robot.py
index 42ce3e0b..a0d95164 100644
--- a/airo-robots/airo_robots/drives/mobile_robot.py
+++ b/airo-robots/airo_robots/drives/mobile_robot.py
@@ -2,6 +2,7 @@
 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.
@@ -26,8 +27,7 @@ class MobileRobot(ABC):
     """
 
     @abstractmethod
-    def set_platform_velocity_target(self, x: float, y: float, a: float, timeout: float,
-                                     align_drives_first: bool = False) -> AwaitableAction:
+    def set_platform_velocity_target(self, x: float, y: float, a: float, timeout: float) -> AwaitableAction:
         """Set the desired platform velocity.
 
         Args:
@@ -35,7 +35,6 @@ def set_platform_velocity_target(self, x: float, y: float, a: float, timeout: fl
             y: Linear velocity along the Y axis.
             a: Angular velocity.
             timeout: After this time, the platform will automatically stop.
-            align_drives_first: Align drives before moving (default: False, will start driving instantly).
 
         Returns:
             An awaitable action."""
@@ -47,3 +46,10 @@ def enable_compliant_mode(self, enabled: bool, compliant_level: CompliantLevel):
         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."""

From 853b0afc95e6201cfc0eaef357fe5cd850fef3fc Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Tue, 27 Aug 2024 08:48:51 +0200
Subject: [PATCH 10/30] Zed coordinate system Z up X forward

Used for Zed's spatial mapping API
---
 airo-camera-toolkit/airo_camera_toolkit/cameras/zed/zed2i.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/airo-camera-toolkit/airo_camera_toolkit/cameras/zed/zed2i.py b/airo-camera-toolkit/airo_camera_toolkit/cameras/zed/zed2i.py
index e7a223eb..1631ee41 100644
--- a/airo-camera-toolkit/airo_camera_toolkit/cameras/zed/zed2i.py
+++ b/airo-camera-toolkit/airo_camera_toolkit/cameras/zed/zed2i.py
@@ -108,6 +108,7 @@ def __init__(  # type: ignore[no-any-unimported]
         # https://www.stereolabs.com/docs/depth-sensing/depth-settings/
         self.camera_params.depth_mode = depth_mode
         self.camera_params.coordinate_units = sl.UNIT.METER
+        self.camera_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Z_UP_X_FWD
         # objects closerby will have artifacts so they are filtered out (querying them will give a - Infinty)
         self.camera_params.depth_minimum_distance = 0.3
         self.camera_params.depth_maximum_distance = 10.0  # filter out far away objects

From 12bd218ac8cf0bf00d632eb7b47041e334b69ce0 Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Mon, 2 Sep 2024 12:05:33 +0200
Subject: [PATCH 11/30] Change Zed camera coordinate system

---
 airo-camera-toolkit/airo_camera_toolkit/cameras/zed/zed2i.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/airo-camera-toolkit/airo_camera_toolkit/cameras/zed/zed2i.py b/airo-camera-toolkit/airo_camera_toolkit/cameras/zed/zed2i.py
index 1631ee41..e7a223eb 100644
--- a/airo-camera-toolkit/airo_camera_toolkit/cameras/zed/zed2i.py
+++ b/airo-camera-toolkit/airo_camera_toolkit/cameras/zed/zed2i.py
@@ -108,7 +108,6 @@ def __init__(  # type: ignore[no-any-unimported]
         # https://www.stereolabs.com/docs/depth-sensing/depth-settings/
         self.camera_params.depth_mode = depth_mode
         self.camera_params.coordinate_units = sl.UNIT.METER
-        self.camera_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Z_UP_X_FWD
         # objects closerby will have artifacts so they are filtered out (querying them will give a - Infinty)
         self.camera_params.depth_minimum_distance = 0.3
         self.camera_params.depth_maximum_distance = 10.0  # filter out far away objects

From 90d724181185ad8fa4eebf771473e6ecbdcc58ff Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Tue, 3 Sep 2024 14:39:41 +0200
Subject: [PATCH 12/30] Format code

---
 airo-robots/airo_robots/drives/hardware/kelo_robile.py          | 2 +-
 airo-robots/airo_robots/drives/hardware/manual_robot_testing.py | 2 +-
 airo-robots/airo_robots/drives/mobile_robot.py                  | 2 ++
 3 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile.py b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
index aa003ea9..36365ab6 100644
--- a/airo-robots/airo_robots/drives/hardware/kelo_robile.py
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
@@ -2,7 +2,7 @@
 from functools import partial
 
 from airo_robots.awaitable_action import AwaitableAction
-from airo_robots.drives.mobile_robot import MobileRobot, CompliantLevel
+from airo_robots.drives.mobile_robot import CompliantLevel, MobileRobot
 from airo_tulip.platform_driver import PlatformDriverType
 from airo_tulip.server.kelo_robile import KELORobile as KELORobileClient
 from airo_typing import Vector3DType
diff --git a/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py b/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py
index b74a50f7..9a21577a 100644
--- a/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py
+++ b/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py
@@ -1,7 +1,7 @@
 """code for manual testing of mobile robot base class implementations.
 """
-from airo_robots.drives.mobile_robot import MobileRobot
 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:
diff --git a/airo-robots/airo_robots/drives/mobile_robot.py b/airo-robots/airo_robots/drives/mobile_robot.py
index a0d95164..660fd89e 100644
--- a/airo-robots/airo_robots/drives/mobile_robot.py
+++ b/airo-robots/airo_robots/drives/mobile_robot.py
@@ -4,11 +4,13 @@
 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

From 827cb347c1b8987927673a30b32a3b323e2a7fe2 Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Tue, 3 Sep 2024 15:25:06 +0200
Subject: [PATCH 13/30] Add documentation

---
 airo-robots/airo_robots/drives/hardware/kelo_robile.py    | 8 +++++---
 .../airo_robots/drives/hardware/kelo_robile_setup.md      | 1 +
 2 files changed, 6 insertions(+), 3 deletions(-)
 create mode 100644 airo-robots/airo_robots/drives/hardware/kelo_robile_setup.md

diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile.py b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
index 36365ab6..58a85a51 100644
--- a/airo-robots/airo_robots/drives/hardware/kelo_robile.py
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
@@ -15,14 +15,16 @@ class KELORobile(MobileRobot):
     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):
+    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.
+        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."""
+            robot_port: Port to connect on (default: 49789)."""
         self._kelo_robile = KELORobileClient(robot_ip, robot_port)
 
     def align_drives(self, x: float, y: float, a: float, timeout: float = 1.0) -> AwaitableAction:
diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile_setup.md b/airo-robots/airo_robots/drives/hardware/kelo_robile_setup.md
new file mode 100644
index 00000000..46b63ebe
--- /dev/null
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile_setup.md
@@ -0,0 +1 @@
+For information on how to set up the KELO Robile, please refer to the documentation of [airo-tulip](https://pypi.org/project/airo-tulip/).

From d68447d4ba3a485e4e41a25c28f94d89103c7445 Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Wed, 4 Sep 2024 09:17:18 +0200
Subject: [PATCH 14/30] Higher frequency awaitable loop for KELORobile commands

---
 .../drives/hardware/kelo_robile.py            | 22 ++++++++++---------
 1 file changed, 12 insertions(+), 10 deletions(-)

diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile.py b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
index 58a85a51..e2d81d39 100644
--- a/airo-robots/airo_robots/drives/hardware/kelo_robile.py
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
@@ -1,5 +1,4 @@
 import time
-from functools import partial
 
 from airo_robots.awaitable_action import AwaitableAction
 from airo_robots.drives.mobile_robot import CompliantLevel, MobileRobot
@@ -43,19 +42,22 @@ def align_drives(self, x: float, y: float, a: float, timeout: float = 1.0) -> Aw
             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)
 
-        def aligned_awaitable(start_time: float) -> bool:
-            return self._kelo_robile.are_drives_aligned() or time.time() > start_time + timeout
-
-        return AwaitableAction(partial(aligned_awaitable, time.time()))
+        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)
 
-        def timeout_awaitable() -> bool:
-            time.sleep(timeout)
-            return True
-
-        return AwaitableAction(timeout_awaitable)
+        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 enable_compliant_mode(self, enabled: bool, compliant_level: CompliantLevel = CompliantLevel.COMPLIANT_WEAK):
         if enabled:

From 6273631efb9f2ccba644e500752a3d0f024481fb Mon Sep 17 00:00:00 2001
From: bitscuity <thomas@bitscuit.be>
Date: Thu, 5 Sep 2024 15:27:38 +0200
Subject: [PATCH 15/30] add position control for mobile robots

---
 .../drives/hardware/kelo_robile.py            | 28 +++++++++++++++++++
 .../drives/hardware/manual_robot_testing.py   |  3 ++
 .../airo_robots/drives/mobile_robot.py        | 13 +++++++++
 3 files changed, 44 insertions(+)

diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile.py b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
index e2d81d39..549210e0 100644
--- a/airo-robots/airo_robots/drives/hardware/kelo_robile.py
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
@@ -1,5 +1,7 @@
+import math
 import time
 
+import numpy as np
 from airo_robots.awaitable_action import AwaitableAction
 from airo_robots.drives.mobile_robot import CompliantLevel, MobileRobot
 from airo_tulip.platform_driver import PlatformDriverType
@@ -59,6 +61,32 @@ def set_platform_velocity_target(self, x: float, y: float, a: float, timeout: fl
             default_sleep_resolution=0.002,
         )
 
+    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()
+
+        def control_loop():
+            current_pose = self._kelo_robile.get_odometry()
+            delta_pose = target_pose - current_pose
+
+            vel_vec_angle = np.atan2(delta_pose[1], delta_pose[0]) - current_pose[2]
+            vel_vec_norm = min(np.linalg.norm(delta_pose[:1]), 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.atan2(np.sin(delta_pose[2]), np.cos(delta_pose[2]))
+            vel_a = max(min(delta_angle, math.pi / 8), -math.pi / 8)
+
+            self._kelo_robile.set_platform_velocity_target(vel_x, vel_y, vel_a, timeout=timeout)
+
+            return time.time_ns() - action_start_time > timeout * 1e9
+
+        return AwaitableAction(
+            control_loop,
+            default_timeout=2 * timeout,
+            default_sleep_resolution=0.002,
+        )
+
     def enable_compliant_mode(self, enabled: bool, compliant_level: CompliantLevel = CompliantLevel.COMPLIANT_WEAK):
         if enabled:
             if compliant_level == CompliantLevel.COMPLIANT_WEAK:
diff --git a/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py b/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py
index 9a21577a..061a1731 100644
--- a/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py
+++ b/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py
@@ -21,3 +21,6 @@ def manually_test_robot_implementation(robot: MobileRobot) -> None:
 
     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 the pose (30 cm, 60 cm, pi/4 rad)")
+    robot.move_platform_to_pose(0.3, 0.6, 3.14 / 4, 5.0).wait()
diff --git a/airo-robots/airo_robots/drives/mobile_robot.py b/airo-robots/airo_robots/drives/mobile_robot.py
index 660fd89e..df156f0e 100644
--- a/airo-robots/airo_robots/drives/mobile_robot.py
+++ b/airo-robots/airo_robots/drives/mobile_robot.py
@@ -41,6 +41,19 @@ def set_platform_velocity_target(self, x: float, y: float, a: float, timeout: fl
         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):
         """Enable compliant mode on the robot.

From d482fae97b66e3b07fe66dac09d839219dfa8d03 Mon Sep 17 00:00:00 2001
From: bitscuity <thomas@bitscuit.be>
Date: Thu, 5 Sep 2024 16:42:20 +0200
Subject: [PATCH 16/30] fix position control issues

---
 .../drives/hardware/kelo_robile.py            | 20 +++++++----
 .../drives/hardware/manual_robot_testing.py   | 35 +++++++++++++------
 2 files changed, 38 insertions(+), 17 deletions(-)

diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile.py b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
index 549210e0..ecefb716 100644
--- a/airo-robots/airo_robots/drives/hardware/kelo_robile.py
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
@@ -64,22 +64,30 @@ def set_platform_velocity_target(self, x: float, y: float, a: float, timeout: fl
     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
 
         def control_loop():
             current_pose = self._kelo_robile.get_odometry()
             delta_pose = target_pose - current_pose
 
-            vel_vec_angle = np.atan2(delta_pose[1], delta_pose[0]) - current_pose[2]
-            vel_vec_norm = min(np.linalg.norm(delta_pose[:1]), 0.5)
+            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.atan2(np.sin(delta_pose[2]), np.cos(delta_pose[2]))
-            vel_a = max(min(delta_angle, math.pi / 8), -math.pi / 8)
+            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)
 
-            self._kelo_robile.set_platform_velocity_target(vel_x, vel_y, vel_a, timeout=timeout)
+            command_timeout = (action_timeout_time - time.time_ns()) * 1e-9
+            self._kelo_robile.set_platform_velocity_target(vel_x, vel_y, vel_a, timeout=command_timeout)
 
-            return time.time_ns() - action_start_time > timeout * 1e9
+            at_target_pose = np.linalg.norm(delta_pose) < 0.01
+            stop = at_target_pose or time.time_ns() - action_start_time > timeout * 1e9
+
+            if stop:
+                self._kelo_robile.set_platform_velocity_target(0.0, 0.0, 0.0)
+
+            return stop
 
         return AwaitableAction(
             control_loop,
diff --git a/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py b/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py
index 061a1731..aa760fb4 100644
--- a/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py
+++ b/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py
@@ -9,18 +9,31 @@ def manually_test_robot_implementation(robot: MobileRobot) -> None:
         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 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 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 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 return to original position")
+    # robot.set_platform_velocity_target(-0.1, -0.1, 0.0, 1.0).wait()
 
-    input("robot will now move to the pose (30 cm, 60 cm, pi/4 rad)")
-    robot.move_platform_to_pose(0.3, 0.6, 3.14 / 4, 5.0).wait()
+    # robot.enable_compliant_mode(True, CompliantLevel.COMPLIANT_STRONG)
+
+    robot.move_platform_to_pose(1.2, 0.0, 0.0, 10.0).wait()
+    print(robot.get_odometry())
+
+    robot.move_platform_to_pose(1.8, 0.6, 3.14 / 2, 10.0).wait()
+    print(robot.get_odometry())
+
+    robot.move_platform_to_pose(1.8, 1.0, 3.14 / 2, 10.0).wait()
+    print(robot.get_odometry())
+
+
+if __name__ == "__main__":
+    mobi = KELORobile("10.10.129.21")
+    manually_test_robot_implementation(mobi)

From a9b02389086c526bc32c26f0b7f40efeb83c3596 Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Fri, 6 Sep 2024 08:51:37 +0200
Subject: [PATCH 17/30] Restore manual_robot_testing

---
 .../drives/hardware/manual_robot_testing.py   | 30 +++++++------------
 .../airo_robots/drives/mobile_robot.py        |  2 +-
 2 files changed, 12 insertions(+), 20 deletions(-)

diff --git a/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py b/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py
index aa760fb4..2e5c46c1 100644
--- a/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py
+++ b/airo-robots/airo_robots/drives/hardware/manual_robot_testing.py
@@ -9,29 +9,21 @@ def manually_test_robot_implementation(robot: MobileRobot) -> None:
         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 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 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 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 return to original position")
+    robot.set_platform_velocity_target(-0.1, -0.1, 0.0, 1.0).wait()
 
-    # robot.enable_compliant_mode(True, CompliantLevel.COMPLIANT_STRONG)
-
-    robot.move_platform_to_pose(1.2, 0.0, 0.0, 10.0).wait()
-    print(robot.get_odometry())
-
-    robot.move_platform_to_pose(1.8, 0.6, 3.14 / 2, 10.0).wait()
-    print(robot.get_odometry())
-
-    robot.move_platform_to_pose(1.8, 1.0, 3.14 / 2, 10.0).wait()
-    print(robot.get_odometry())
+    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__":
diff --git a/airo-robots/airo_robots/drives/mobile_robot.py b/airo-robots/airo_robots/drives/mobile_robot.py
index df156f0e..083f4ee6 100644
--- a/airo-robots/airo_robots/drives/mobile_robot.py
+++ b/airo-robots/airo_robots/drives/mobile_robot.py
@@ -48,7 +48,7 @@ def move_platform_to_pose(self, x: float, y: float, a: float, timeout: float) ->
         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..
+            a: Orientation around the startup pose's Z axis.
             timeout: After this time, the platform will automatically stop.
 
         Returns:

From 083a8e77728e640817c0c703dc3d007fe335a705 Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Tue, 17 Sep 2024 08:52:57 +0200
Subject: [PATCH 18/30] Avoid setting negative timeouts

---
 airo-robots/airo_robots/drives/hardware/kelo_robile.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile.py b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
index ecefb716..6a628021 100644
--- a/airo-robots/airo_robots/drives/hardware/kelo_robile.py
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
@@ -79,7 +79,8 @@ def control_loop():
             vel_a = max(min(delta_angle, math.pi / 4), -math.pi / 4)
 
             command_timeout = (action_timeout_time - time.time_ns()) * 1e-9
-            self._kelo_robile.set_platform_velocity_target(vel_x, vel_y, vel_a, timeout=command_timeout)
+            if command_timeout >= 0.0:
+                self._kelo_robile.set_platform_velocity_target(vel_x, vel_y, vel_a, timeout=command_timeout)
 
             at_target_pose = np.linalg.norm(delta_pose) < 0.01
             stop = at_target_pose or time.time_ns() - action_start_time > timeout * 1e9

From 276b6a51908dcbb644390b4bfd8ee3916bfd5fbf Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Tue, 17 Sep 2024 10:41:05 +0200
Subject: [PATCH 19/30] Add reset_odometry function

---
 airo-robots/airo_robots/drives/hardware/kelo_robile.py | 3 +++
 airo-robots/airo_robots/drives/mobile_robot.py         | 4 ++++
 airo-robots/setup.py                                   | 2 +-
 3 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile.py b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
index 6a628021..7d951142 100644
--- a/airo-robots/airo_robots/drives/hardware/kelo_robile.py
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
@@ -109,3 +109,6 @@ def enable_compliant_mode(self, enabled: bool, compliant_level: CompliantLevel =
 
     def get_odometry(self) -> Vector3DType:
         return self._kelo_robile.get_odometry()
+
+    def reset_odometry(self):
+        self._kelo_robile.reset_odometry()
diff --git a/airo-robots/airo_robots/drives/mobile_robot.py b/airo-robots/airo_robots/drives/mobile_robot.py
index 083f4ee6..47455ef4 100644
--- a/airo-robots/airo_robots/drives/mobile_robot.py
+++ b/airo-robots/airo_robots/drives/mobile_robot.py
@@ -68,3 +68,7 @@ def get_odometry(self) -> Vector3DType:
         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):
+        """Reset the robot's odometry to `(0, 0, 0)`."""
diff --git a/airo-robots/setup.py b/airo-robots/setup.py
index 492a2986..97a4c6ee 100644
--- a/airo-robots/setup.py
+++ b/airo-robots/setup.py
@@ -15,7 +15,7 @@
         "click",
         "airo-typing",
         "airo-spatial-algebra",
-        "airo-tulip==0.0.6",
+        "airo-tulip==0.0.7",
     ],
     packages=setuptools.find_packages(),
     package_data={"airo_robots": ["py.typed"]},

From 90a6432e3a9c35ce786d2d5c770af435916036ea Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Thu, 19 Sep 2024 09:24:24 +0200
Subject: [PATCH 20/30] Bump airo-tulip version for bug fixes

---
 airo-robots/setup.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/airo-robots/setup.py b/airo-robots/setup.py
index 97a4c6ee..9d4987d9 100644
--- a/airo-robots/setup.py
+++ b/airo-robots/setup.py
@@ -15,7 +15,7 @@
         "click",
         "airo-typing",
         "airo-spatial-algebra",
-        "airo-tulip==0.0.7",
+        "airo-tulip==0.0.8",
     ],
     packages=setuptools.find_packages(),
     package_data={"airo_robots": ["py.typed"]},

From 81925b4d165eaa597ac089b871e3d21ebe6ab135 Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Mon, 30 Sep 2024 14:17:01 +0200
Subject: [PATCH 21/30] Add support for GetVelocity

---
 airo-robots/airo_robots/drives/hardware/kelo_robile.py | 3 +++
 airo-robots/airo_robots/drives/mobile_robot.py         | 8 ++++++++
 airo-robots/setup.py                                   | 2 +-
 3 files changed, 12 insertions(+), 1 deletion(-)

diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile.py b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
index 7d951142..38c39384 100644
--- a/airo-robots/airo_robots/drives/hardware/kelo_robile.py
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
@@ -110,5 +110,8 @@ def enable_compliant_mode(self, enabled: bool, compliant_level: CompliantLevel =
     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):
         self._kelo_robile.reset_odometry()
diff --git a/airo-robots/airo_robots/drives/mobile_robot.py b/airo-robots/airo_robots/drives/mobile_robot.py
index 47455ef4..d8acf412 100644
--- a/airo-robots/airo_robots/drives/mobile_robot.py
+++ b/airo-robots/airo_robots/drives/mobile_robot.py
@@ -72,3 +72,11 @@ def get_odometry(self) -> Vector3DType:
     @abstractmethod
     def reset_odometry(self):
         """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."""
diff --git a/airo-robots/setup.py b/airo-robots/setup.py
index 9d4987d9..ab2c5c11 100644
--- a/airo-robots/setup.py
+++ b/airo-robots/setup.py
@@ -15,7 +15,7 @@
         "click",
         "airo-typing",
         "airo-spatial-algebra",
-        "airo-tulip==0.0.8",
+        "airo-tulip==0.0.9",
     ],
     packages=setuptools.find_packages(),
     package_data={"airo_robots": ["py.typed"]},

From 61d6b4cefe370b899e95a0e4fd6fd8e337be02cc Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Mon, 30 Sep 2024 15:28:50 +0200
Subject: [PATCH 22/30] Add airo-tulip to list of sister repositories

---
 README.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/README.md b/README.md
index cb6a9140..f56f8d19 100644
--- a/README.md
+++ b/README.md
@@ -45,6 +45,7 @@ Repositories that follow the same style as `airo-mono` packages, but are not par
 | 🛒 [`airo-models`](https://github.com/airo-ugent/airo-models)   | Collection of robot and object models and URDFs |
 | 🐉 [`airo-drake`](https://github.com/airo-ugent/airo-drake)     | Integration with Drake                          |
 | 🧭 [`airo-planner`](https://github.com/airo-ugent/airo-planner) | Motion planning interfaces      |
+| 🚗 [`airo-tulip`](https://github.com/airo-ugent/airo-tulip) | Driver for the KELO mobile robot platform |
 
 ### Usage & Philosophy 📖
 We believe in *keep simple things simple*. Starting a new project should\* be as simple as:

From 359fbe5548b540bcb332f4ae182e04856719e97f Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Mon, 7 Oct 2024 15:20:02 +0200
Subject: [PATCH 23/30] Update changelog

---
 CHANGELOG.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index ca5bdf10..09ae2fc8 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -24,6 +24,7 @@ This project uses a [CalVer](https://calver.org/) versioning scheme with monthly
 - `Zed2i.ULTRA_DEPTH_MODE` to enable the ultra depth setting for the Zed2i cameras
 - `OpenCVVideoCapture` implementation of `RGBCamera` for working with arbitrary cameras
 - `MultiprocessRGBRerunLogger` and `MultiprocessRGBDRerunLogger` now allow you to pass an `entity_path` value which determines where the RGB and depth images will be logged
+- `MobileRobot` and `KELORobile` interface and subclass added, to control mobile robots via the `airo-tulip` package
 
 ### Changed
 - `coco-to-yolo` conversion now creates a single polygon of all disconnected parts of the mask instead of simply taking the first polygon of the list.

From 7c295db486c5336fad705771fb8b8658c776d828 Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Mon, 7 Oct 2024 15:34:02 +0200
Subject: [PATCH 24/30] Update dependency for airo-tulip to 0.0.10

---
 airo-robots/setup.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/airo-robots/setup.py b/airo-robots/setup.py
index ab2c5c11..99663f58 100644
--- a/airo-robots/setup.py
+++ b/airo-robots/setup.py
@@ -15,7 +15,7 @@
         "click",
         "airo-typing",
         "airo-spatial-algebra",
-        "airo-tulip==0.0.9",
+        "airo-tulip==0.0.10",
     ],
     packages=setuptools.find_packages(),
     package_data={"airo_robots": ["py.typed"]},

From 3d83198543558d24adda250618817d912ad14d5d Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Mon, 7 Oct 2024 15:53:08 +0200
Subject: [PATCH 25/30] Make MyPy happy

---
 .../airo_robots/drives/hardware/kelo_robile.py       | 12 +++++++-----
 airo-robots/airo_robots/drives/mobile_robot.py       |  4 ++--
 2 files changed, 9 insertions(+), 7 deletions(-)

diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile.py b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
index 38c39384..2f9df8b4 100644
--- a/airo-robots/airo_robots/drives/hardware/kelo_robile.py
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
@@ -4,8 +4,8 @@
 import numpy as np
 from airo_robots.awaitable_action import AwaitableAction
 from airo_robots.drives.mobile_robot import CompliantLevel, MobileRobot
-from airo_tulip.platform_driver import PlatformDriverType
-from airo_tulip.server.kelo_robile import KELORobile as KELORobileClient
+from airo_tulip.platform_driver import PlatformDriverType  # type: ignore
+from airo_tulip.server.kelo_robile import KELORobile as KELORobileClient  # type: ignore
 from airo_typing import Vector3DType
 
 
@@ -66,7 +66,7 @@ def move_platform_to_pose(self, x: float, y: float, a: float, timeout: float) ->
         action_start_time = time.time_ns()
         action_timeout_time = action_start_time + timeout * 1e9
 
-        def control_loop():
+        def control_loop() -> None:
             current_pose = self._kelo_robile.get_odometry()
             delta_pose = target_pose - current_pose
 
@@ -96,7 +96,9 @@ def control_loop():
             default_sleep_resolution=0.002,
         )
 
-    def enable_compliant_mode(self, enabled: bool, compliant_level: CompliantLevel = CompliantLevel.COMPLIANT_WEAK):
+    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)
@@ -113,5 +115,5 @@ def get_odometry(self) -> Vector3DType:
     def get_velocity(self) -> Vector3DType:
         return self._kelo_robile.get_velocity()
 
-    def reset_odometry(self):
+    def reset_odometry(self) -> None:
         self._kelo_robile.reset_odometry()
diff --git a/airo-robots/airo_robots/drives/mobile_robot.py b/airo-robots/airo_robots/drives/mobile_robot.py
index d8acf412..a5f009d1 100644
--- a/airo-robots/airo_robots/drives/mobile_robot.py
+++ b/airo-robots/airo_robots/drives/mobile_robot.py
@@ -55,7 +55,7 @@ def move_platform_to_pose(self, x: float, y: float, a: float, timeout: float) ->
             An awaitable action."""
 
     @abstractmethod
-    def enable_compliant_mode(self, enabled: bool, compliant_level: CompliantLevel):
+    def enable_compliant_mode(self, enabled: bool, compliant_level: CompliantLevel) -> None:
         """Enable compliant mode on the robot.
 
         Args:
@@ -70,7 +70,7 @@ def get_odometry(self) -> Vector3DType:
         Returns: A 3D vector with a value for the `x` position, `y` position, and `theta` angle of the robot."""
 
     @abstractmethod
-    def reset_odometry(self):
+    def reset_odometry(self) -> None:
         """Reset the robot's odometry to `(0, 0, 0)`."""
 
     @abstractmethod

From 56ce2ef5a29ad144bdbc76eb2a68b6ec0f0db259 Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Mon, 7 Oct 2024 15:59:08 +0200
Subject: [PATCH 26/30] AwaitableAction control_loop returns a boolean, not
 None

---
 airo-robots/airo_robots/drives/hardware/kelo_robile.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile.py b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
index 2f9df8b4..d390d14c 100644
--- a/airo-robots/airo_robots/drives/hardware/kelo_robile.py
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
@@ -66,7 +66,7 @@ def move_platform_to_pose(self, x: float, y: float, a: float, timeout: float) ->
         action_start_time = time.time_ns()
         action_timeout_time = action_start_time + timeout * 1e9
 
-        def control_loop() -> None:
+        def control_loop() -> bool:
             current_pose = self._kelo_robile.get_odometry()
             delta_pose = target_pose - current_pose
 

From 12a24c49f51b311d38a7fd5abfff42a7077e71c8 Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Mon, 7 Oct 2024 16:06:09 +0200
Subject: [PATCH 27/30] Force scalar bool from NumPy based on assumptions that
 norm returns a scalar

---
 airo-robots/airo_robots/drives/hardware/kelo_robile.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile.py b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
index d390d14c..03dd8f02 100644
--- a/airo-robots/airo_robots/drives/hardware/kelo_robile.py
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
@@ -82,7 +82,7 @@ def control_loop() -> bool:
             if command_timeout >= 0.0:
                 self._kelo_robile.set_platform_velocity_target(vel_x, vel_y, vel_a, timeout=command_timeout)
 
-            at_target_pose = np.linalg.norm(delta_pose) < 0.01
+            at_target_pose = bool(np.linalg.norm(delta_pose) < 0.01)
             stop = at_target_pose or time.time_ns() - action_start_time > timeout * 1e9
 
             if stop:

From 8725cdce8366251be983cc3307f8b78d1b698448 Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Mon, 14 Oct 2024 09:28:34 +0200
Subject: [PATCH 28/30] Update to airo-tulip 0.1.0

---
 airo-robots/airo_robots/drives/hardware/kelo_robile.py | 4 ++--
 airo-robots/setup.py                                   | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile.py b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
index 03dd8f02..acaa94ed 100644
--- a/airo-robots/airo_robots/drives/hardware/kelo_robile.py
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
@@ -4,8 +4,8 @@
 import numpy as np
 from airo_robots.awaitable_action import AwaitableAction
 from airo_robots.drives.mobile_robot import CompliantLevel, MobileRobot
-from airo_tulip.platform_driver import PlatformDriverType  # type: ignore
-from airo_tulip.server.kelo_robile import KELORobile as KELORobileClient  # type: ignore
+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
 
 
diff --git a/airo-robots/setup.py b/airo-robots/setup.py
index 99663f58..56b68c30 100644
--- a/airo-robots/setup.py
+++ b/airo-robots/setup.py
@@ -15,7 +15,7 @@
         "click",
         "airo-typing",
         "airo-spatial-algebra",
-        "airo-tulip==0.0.10",
+        "airo-tulip==0.1.0",
     ],
     packages=setuptools.find_packages(),
     package_data={"airo_robots": ["py.typed"]},

From a0e94f5003d2f23edf2288ab088267e97a351f98 Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Wed, 16 Oct 2024 16:50:26 +0200
Subject: [PATCH 29/30] Run position control loop in separate thread

---
 .../drives/hardware/kelo_robile.py            | 34 +++++++++++++------
 1 file changed, 24 insertions(+), 10 deletions(-)

diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile.py b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
index acaa94ed..e1f4a879 100644
--- a/airo-robots/airo_robots/drives/hardware/kelo_robile.py
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
@@ -1,5 +1,6 @@
 import math
 import time
+from threading import Thread
 
 import numpy as np
 from airo_robots.awaitable_action import AwaitableAction
@@ -26,8 +27,12 @@ def __init__(self, robot_ip: str, robot_port: int = 49789):
         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.
 
@@ -61,12 +66,11 @@ def set_platform_velocity_target(self, x: float, y: float, a: float, timeout: fl
             default_sleep_resolution=0.002,
         )
 
-    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
-
-        def control_loop() -> bool:
+    def _move_platform_to_pose_control_loop(
+        self, target_pose: Vector3DType, action_start_time: float, action_timeout_time: float, timeout: float
+    ):
+        stop = False
+        while not stop:
             current_pose = self._kelo_robile.get_odometry()
             delta_pose = target_pose - current_pose
 
@@ -85,13 +89,23 @@ def control_loop() -> bool:
             at_target_pose = bool(np.linalg.norm(delta_pose) < 0.01)
             stop = at_target_pose or time.time_ns() - action_start_time > timeout * 1e9
 
-            if stop:
-                self._kelo_robile.set_platform_velocity_target(0.0, 0.0, 0.0)
+        self._kelo_robile.set_platform_velocity_target(0.0, 0.0, 0.0)
+        self._control_loop_done = True
 
-            return stop
+    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(
-            control_loop,
+            lambda: self._control_loop_done,
             default_timeout=2 * timeout,
             default_sleep_resolution=0.002,
         )

From a3034973b9ae70160e3694acc6bb22e05baf8ec2 Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mth.decoster@gmail.com>
Date: Wed, 16 Oct 2024 16:54:44 +0200
Subject: [PATCH 30/30] Add return type annotation -> None (mypy)

---
 airo-robots/airo_robots/drives/hardware/kelo_robile.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/airo-robots/airo_robots/drives/hardware/kelo_robile.py b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
index e1f4a879..21afaacc 100644
--- a/airo-robots/airo_robots/drives/hardware/kelo_robile.py
+++ b/airo-robots/airo_robots/drives/hardware/kelo_robile.py
@@ -68,7 +68,7 @@ def set_platform_velocity_target(self, x: float, y: float, a: float, timeout: fl
 
     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()