diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/controller.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/controller.py index 1d2b7ab7f..b6cc87a08 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/controller.py +++ b/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 diff --git a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml index 7dac8dd47..f165243df 100644 --- a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml +++ b/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] - \ No newline at end of file diff --git a/bitbots_navigation/bitbots_path_planning/test/__snapshots__/test_controller.ambr b/bitbots_navigation/bitbots_path_planning/test/__snapshots__/test_controller.ambr index 48d35b4a3..620fc20d7 100644 --- a/bitbots_navigation/bitbots_path_planning/test/__snapshots__/test_controller.ambr +++ b/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))' # --- diff --git a/bitbots_navigation/bitbots_path_planning/test/test_controller.py b/bitbots_navigation/bitbots_path_planning/test/test_controller.py index 78253a97b..57a2e7916 100644 --- a/bitbots_navigation/bitbots_path_planning/test/test_controller.py +++ b/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,8 +95,15 @@ 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 @@ -96,11 +111,11 @@ def test_step_limits_rotation(node, tf2_buffer, config, pose_left_line, pose_rig 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):