Limit diagonal speed in path planning (#534)

Author: Flova

bitbots_navigation/bitbots_path_planning/bitbots_path_planning/controller.py

@@ -108,35 +108,40 @@ def step(self, path: Path) -> tuple[Twist, PointStamped]:
             self.node.config.controller.max_rotation_vel,
         )
 
+        # Calculate the heading of our linear velocity vector in the local frame of the robot
+        local_heading = walk_angle - self._get_yaw(current_pose)
+
+        # Convert the heading of the robot to a unit vector
+        local_heading_vector_x = math.cos(local_heading)
+        local_heading_vector_y = math.sin(local_heading)
+
+        # Returns interpolates the x and y maximum velocity linearly based on our heading
+        # See https://github.com/bit-bots/bitbots_main/issues/366#issuecomment-2161460917
+        def interpolate_max_vel(x_limit, y_limit, target_x, target_y):
+            n = (x_limit * abs(target_y)) + y_limit * target_x
+            intersection_x = (x_limit * y_limit * target_x) / n
+            intersection_y = (x_limit * y_limit * abs(target_y)) / n
+            return math.hypot(intersection_x, intersection_y)
+
+        # Limit the x and y velocity with the heading specific maximum velocity
+        walk_vel = min(
+            walk_vel,
+            # Calc the max velocity for the current heading
+            interpolate_max_vel(
+                # Use different maximum values for forward and backwards x movement
+                self.node.config.controller.max_vel_x
+                if local_heading_vector_x > 0
+                else self.node.config.controller.min_vel_x,
+                self.node.config.controller.max_vel_y,
+                local_heading_vector_x,
+                local_heading_vector_y,
+            ),
+        )
+
         # Calculate the x and y components of our linear velocity based on the desired heading and the
         # desired translational velocity.
-        cmd_vel.linear.x = math.cos(walk_angle - self._get_yaw(current_pose)) * walk_vel
-        cmd_vel.linear.y = math.sin(walk_angle - self._get_yaw(current_pose)) * walk_vel
-
-        # Scale command accordingly if a limit is exceeded
-        if cmd_vel.linear.x > self.node.config.controller.max_vel_x:
-            self.node.get_logger().debug(
-                f"X max LIMIT reached: {cmd_vel.linear.x} > {self.node.config.controller.max_vel_x}, with Y being {cmd_vel.linear.y}"
-            )
-            cmd_vel.linear.y *= self.node.config.controller.max_vel_x / cmd_vel.linear.x
-            cmd_vel.linear.x = self.node.config.controller.max_vel_x
-            self.node.get_logger().debug(f"X max LIMIT reached: scale Y to {cmd_vel.linear.y}")
-
-        if cmd_vel.linear.x < self.node.config.controller.min_vel_x:
-            self.node.get_logger().debug(
-                f"X min LIMIT reached: {cmd_vel.linear.x} < {self.node.config.controller.min_vel_x}, with Y being {cmd_vel.linear.y}"
-            )
-            cmd_vel.linear.y *= self.node.config.controller.min_vel_x / cmd_vel.linear.x
-            cmd_vel.linear.x = self.node.config.controller.min_vel_x
-            self.node.get_logger().debug(f"X min LIMIT reached: scale Y to {cmd_vel.linear.y}")
-
-        if abs(cmd_vel.linear.y) > self.node.config.controller.max_vel_y:
-            self.node.get_logger().debug(
-                f"Y max LIMIT reached: {cmd_vel.linear.y} > {self.node.config.controller.max_vel_y}, with X being {cmd_vel.linear.x}"
-            )
-            cmd_vel.linear.x *= self.node.config.controller.max_vel_y / abs(cmd_vel.linear.y)
-            cmd_vel.linear.y = math.copysign(self.node.config.controller.max_vel_y, cmd_vel.linear.y)
-            self.node.get_logger().debug(f"Y max LIMIT reached: scale X to {cmd_vel.linear.x}")
+        cmd_vel.linear.x = local_heading_vector_x * walk_vel
+        cmd_vel.linear.y = local_heading_vector_y * walk_vel
 
         # Apply the desired rotational velocity
         cmd_vel.angular.z = rot_goal_vel

bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml

@@ -1,5 +1,5 @@
 bitbots_path_planning:
-  base_footprint_frame: 
+  base_footprint_frame:
     type: string
     default_value: base_footprint
     description: 'The frame of the robot base'
@@ -16,7 +16,7 @@ bitbots_path_planning:
     description: 'The rate at which the path planning is executed'
     validation:
       bounds<>: [0.0, 100.0]
-  
+
   map:
     planning_frame:
       type: string
@@ -86,7 +86,7 @@ bitbots_path_planning:
 
     max_vel_x:
       type: double
-      default_value: 0.15
+      default_value: 0.12
       description: 'Maximum velocity we want to reach in different directions (base_footprint coordinate system)'
       validation:
         bounds<>: [0.0, 1.0]
@@ -132,4 +132,3 @@ bitbots_path_planning:
       description: 'Distance at which we switch from orienting towards the path to orienting towards the goal poses orientation (in meters)'
       validation:
         bounds<>: [0.0, 10.0]
-      

bitbots_navigation/bitbots_path_planning/test/__snapshots__/test_controller.ambr

@@ -3,7 +3,7 @@
   dict({
     'carrot_distance': 20,
     'max_rotation_vel': 0.4,
-    'max_vel_x': 0.15,
+    'max_vel_x': 0.12,
     'max_vel_y': 0.1,
     'min_vel_x': -0.1,
     'orient_to_goal_distance': 1.0,
@@ -14,5 +14,5 @@
   })
 # ---
 # name: test_step_cmd_vel_smoothing
-  'geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.15, y=0.09999999999999996, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.20666164469042259))'
+  'geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.073884548439286, y=0.051963071290922924, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.20666164469042259))'
 # ---

bitbots_navigation/bitbots_path_planning/test/test_controller.py

@@ -1,3 +1,4 @@
+import math
 from unittest.mock import Mock
 
 import pytest
@@ -37,7 +38,7 @@ def test_step_limits_forward_x_velocity(node, tf2_buffer, config, pose_opponent_
 
     controller.step(path_to(pose_opponent_goal))
 
-    assert controller.last_cmd_vel.linear.x == config.controller.max_vel_x
+    assert controller.last_cmd_vel.linear.x == pytest.approx(config.controller.max_vel_x)
     assert controller.last_cmd_vel.linear.y == pytest.approx(0)
     assert controller.last_cmd_vel.linear.z == 0
 
@@ -47,7 +48,7 @@ def test_step_limits_backward_x_velocity(node, tf2_buffer, config, pose_own_goal
 
     controller.step(path_to(pose_own_goal))
 
-    assert controller.last_cmd_vel.linear.x == config.controller.min_vel_x
+    assert controller.last_cmd_vel.linear.x == pytest.approx(config.controller.min_vel_x)
     assert controller.last_cmd_vel.linear.y == pytest.approx(0)
     assert controller.last_cmd_vel.linear.z == 0
 
@@ -58,7 +59,7 @@ def test_step_limits_forward_y_velocity(node, tf2_buffer, config, pose_left_line
     controller.step(path_to(pose_left_line))
 
     assert controller.last_cmd_vel.linear.x == pytest.approx(0)
-    assert controller.last_cmd_vel.linear.y == config.controller.max_vel_y
+    assert controller.last_cmd_vel.linear.y == pytest.approx(config.controller.max_vel_y)
     assert controller.last_cmd_vel.linear.z == 0
 
 
@@ -68,7 +69,7 @@ def test_step_limits_backward_y_velocity(node, tf2_buffer, config, pose_right_li
     controller.step(path_to(pose_right_line))
 
     assert controller.last_cmd_vel.linear.x == pytest.approx(0)
-    assert controller.last_cmd_vel.linear.y == -config.controller.max_vel_y
+    assert controller.last_cmd_vel.linear.y == pytest.approx(-config.controller.max_vel_y)
     assert controller.last_cmd_vel.linear.z == 0
 
 
@@ -77,8 +78,15 @@ def test_step_limits_forward_xy_velocities(node, tf2_buffer, config, pose_oppone
 
     controller.step(path_to(pose_opponent_corner))
 
-    assert controller.last_cmd_vel.linear.x == pytest.approx(config.controller.max_vel_x)
-    assert controller.last_cmd_vel.linear.y == pytest.approx(config.controller.max_vel_y)
+    goal_heading_angle = math.atan2(pose_opponent_corner.pose.position.y, pose_opponent_corner.pose.position.x)
+    walk_command_angle = math.atan2(controller.last_cmd_vel.linear.y, controller.last_cmd_vel.linear.x)
+
+    assert goal_heading_angle == pytest.approx(walk_command_angle)
+
+    walk_command_speed = math.hypot(controller.last_cmd_vel.linear.x, controller.last_cmd_vel.linear.y)
+
+    assert walk_command_speed < config.controller.max_vel_x
+    assert walk_command_speed < config.controller.max_vel_y
     assert controller.last_cmd_vel.linear.z == 0
 
 
@@ -87,20 +95,27 @@ def test_step_limits_backward_xy_velocities(node, tf2_buffer, config, pose_own_c
 
     controller.step(path_to(pose_own_corner))
 
-    assert controller.last_cmd_vel.linear.x == pytest.approx(config.controller.min_vel_x)
-    assert controller.last_cmd_vel.linear.y == pytest.approx(-0.066666666)
+    goal_heading_angle = math.atan2(pose_own_corner.pose.position.y, pose_own_corner.pose.position.x)
+    walk_command_angle = math.atan2(controller.last_cmd_vel.linear.y, controller.last_cmd_vel.linear.x)
+
+    assert goal_heading_angle == pytest.approx(walk_command_angle)
+
+    walk_command_speed = math.hypot(controller.last_cmd_vel.linear.x, controller.last_cmd_vel.linear.y)
+
+    assert walk_command_speed < abs(config.controller.min_vel_x)
+    assert walk_command_speed < config.controller.max_vel_y
     assert controller.last_cmd_vel.linear.z == 0
 
 
 def test_step_limits_rotation(node, tf2_buffer, config, pose_left_line, pose_right_line):
     controller = setup_controller(node, tf2_buffer)
 
     controller.step(path_to(pose_left_line))
-    assert controller.last_cmd_vel.angular.z == config.controller.max_rotation_vel
+    assert controller.last_cmd_vel.angular.z == pytest.approx(config.controller.max_rotation_vel)
 
     controller.last_update_time = None
     controller.step(path_to(pose_right_line))
-    assert controller.last_cmd_vel.angular.z == -config.controller.max_rotation_vel
+    assert controller.last_cmd_vel.angular.z == pytest.approx(-config.controller.max_rotation_vel)
 
 
 def test_step_cmd_vel_smoothing(snapshot, node, tf2_buffer, config, pose_opponent_corner):