fix position control issues

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,

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)