From 355b90489b8051fb3c408209e118cd6a15e7364a Mon Sep 17 00:00:00 2001 From: Kostubh Khandelwal Date: Thu, 26 Sep 2024 12:43:44 +0000 Subject: [PATCH 1/3] Added Vector Pursuit Controller Signed-off-by: Kostubh Khandelwal --- configuration/index.rst | 1 + .../configuring-vector-pursuit-controller.rst | 394 ++++++++++++++++++ plugins/index.rst | 61 +-- setup_guides/algorithm/select_algorithm.rst | 4 + tuning/index.rst | 4 + 5 files changed, 436 insertions(+), 28 deletions(-) create mode 100644 configuration/packages/configuring-vector-pursuit-controller.rst diff --git a/configuration/index.rst b/configuration/index.rst index aaff8e8ad..ba9f1767e 100644 --- a/configuration/index.rst +++ b/configuration/index.rst @@ -21,6 +21,7 @@ the best navigation performance. packages/configuring-controller-server.rst packages/configuring-dwb-controller.rst packages/configuring-regulated-pp.rst + packages/configuring-vector-pursuit-controller.rst packages/configuring-mppic.rst packages/configuring-rotation-shim-controller.rst packages/configuring-graceful-motion-controller.rst diff --git a/configuration/packages/configuring-vector-pursuit-controller.rst b/configuration/packages/configuring-vector-pursuit-controller.rst new file mode 100644 index 000000000..2fd2f2bdb --- /dev/null +++ b/configuration/packages/configuring-vector-pursuit-controller.rst @@ -0,0 +1,394 @@ +.. _configuring_vector_pursuit_controller: + +Vector Pursuit Controller +######################### + +Source code on Github_. + +.. _Github: https://github.com/blackcoffeerobotics/vector_pursuit_controller + +The Vector Pursuit controller implements a path-tracking algorithm based on the theory of screws. Unlike most other controllers, Vector Pursuit incorporates the path's orientation with the position when calculating velocity commands. +The use of screws unifies rotation and translation, making the controller more robust and geometrically meaningful, especially in complex, dynamic environments. This enables the robot to follow the path more smoothly and accurately, especially when navigating tight corners or moving at high speeds. +By adjusting linear velocities based on path curvature, the controller minimizes overshoot in sharp turns, enhancing safety and stability. + +See the package's ``README`` for more complete information. + + +Vector Pursuit Parameters +********************************* + +:k: + + ============== =========================== + Type Default + -------------- --------------------------- + double 8.0 + ============== =========================== + + Description + k is a constant that relates the rotation and translation times when calculating the resultant screw(as a sum of the rotation and translation screws). The relationship is rotation_time = k * translation_time. Increasing k will put more weightage on faster translation and decreasing k will, in turn, apply more weightage on faster rotation. + +:desired_linear_vel: + + ============== =========================== + Type Default + -------------- --------------------------- + double 0.5 + ============== =========================== + + Description + The desired maximum linear velocity (m/s). +:min_turning_radius: + + ============== =========================== + Type Default + -------------- --------------------------- + double 1.0 + ============== =========================== + + Description + The minimum achievable turning radius(m) of the robot. Remember, sharper turns have smaller radii. + +:lookahead_dist: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.6 + ============== ============================= + + Description + The lookahead distance (m) to find the lookahead point. + +:min_lookahead_dist: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.3 + ============== ============================= + + Description + The minimum lookahead distance (m) threshold for adaptive lookahead point. + +:max_lookahead_dist: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.9 + ============== ============================= + + Description + The maximum lookahead distance (m) threshold for adaptive lookahead point. + +:lookahead_time: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 1.5 + ============== ============================= + + Description + The time (s) to integrate the current linear velocity to get the scaled lookahead distance for adaptive lookahead point. + +:rotate_to_heading_angular_vel: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 1.8 + ============== ============================= + + Description + Angular velocity for rotating to heading. + +:transform_tolerance: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.1 + ============== ============================= + + Description + The TF transform tolerance (s). + +:use_velocity_scaled_lookahead_dist: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool false + ============== ============================= + + Description + Enable velocity adaptive ``lookahead_distance``. + +:min_approach_linear_velocity: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.05 + ============== ============================= + + Description + The minimum velocity approach aware linear velocity scaling can produce. + +:approach_velocity_scaling_dist: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 1.0 + ============== ============================= + + Description + The distance to goal at which velocity scaling will begin. Set to 0 to disable. + +:use_collision_detection: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool true + ============== ============================= + + Description + Enable/disable collision detection. + +:max_allowed_time_to_collision_up_to_target: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 1.0 + ============== ============================= + + Description + Maximum time (s) allowed for collision checking. + +:use_cost_regulated_linear_velocity_scaling: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool true + ============== ============================= + + Description + Whether to slowdown in close proximity to obstacles. + +:cost_scaling_dist: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.6 + ============== ============================= + + Description + Distance for cost-based velocity scaling. + +:cost_scaling_gain: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 1.0 + ============== ============================= + + Description + Multiplicative factor for cost-based velocity scaling. + +:inflation_cost_scaling_factor: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 3.0 + ============== ============================= + + Description + Factor for inflation cost scaling. + +:min_linear_velocity: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.05 + ============== ============================= + + Description + The minimum speed (m/s) robot must run at. Must be ``> 0.01``. + +:use_rotate_to_heading: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool true + ============== ============================= + + Description + Enable/disable rotate-to-heading behavior. Will override reversing if both are enabled. + +:allow_reversing: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool false + ============== ============================= + + Description + Enable/Disable reversing movement. Will move in reverse if the lookahead point is behind the robot. + +:rotate_to_heading_min_angle: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.785 + ============== ============================= + + Description + The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place, if ``use_rotate_to_heading`` is ``true``. + +:max_angular_accel: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 3.2 + ============== ============================= + + Description + Maximum allowable angular acceleration (rad/s^2). + +:max_linear_accel: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 2.0 + ============== ============================= + + Description + Maximum allowable linear acceleration (m/s^2). + +:max_lateral_accel: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.5 + ============== ============================= + + Description + Maximum allowable acceleration (m/s^2) perpendicular to axis of desired translation motion. This limit is used to restrict the maximum speed for a given path curvature. + +:use_interpolation: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool true + ============== ============================= + + Description + Calculate lookahead point exactly at the lookahead distance. Otherwise select a discrete point on the path. + +:use_heading_from_path: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool false + ============== ============================= + + Description + If set to true, uses the orientation from the path poses otherwise compute appropriate orientations. Only set to true if using a planner that takes robot heading into account like Smac Planner. + +:max_robot_pose_search_dist: + + ============== ================================================= + Type Default + -------------- ------------------------------------------------- + double Local costmap max extent (max(width, height) / 2) + ============== ================================================= + + Description + Maximum search distance for target poses along the global plan. + +Example +******* +.. code-block:: yaml + + controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.25 + movement_time_allowance: 10.0 + + # Goal checker parameters + general_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + + FollowPath: + plugin: "vector_pursuit_controller::VectorPursuitController" + k: 5.0 + desired_linear_vel: 0.5 + min_turning_radius: 0.25 + lookahead_dist: 1.0 + min_lookahead_dist: 0.5 + max_lookahead_dist: 1.5 + lookahead_time: 1.5 + rotate_to_heading_angular_vel: 0.5 + transform_tolerance: 0.1 + use_velocity_scaled_lookahead_dist: false + min_linear_velocity: 0.0 + min_approach_linear_velocity: 0.05 + approach_velocity_scaling_dist: 0.5 + max_allowed_time_to_collision_up_to_target: 1.0 + use_collision_detection: true + use_cost_regulated_linear_velocity_scaling: true + cost_scaling_dist: 0.5 + cost_scaling_gain: 1.0 + inflation_cost_scaling_factor: 3.0 + use_rotate_to_heading: true + allow_reversing: false + rotate_to_heading_min_angle: 0.5 + max_angular_accel: 3.0 + max_linear_accel: 2.0 + max_lateral_accel: 0.2 + max_robot_pose_search_dist: 10.0 + use_interpolation: true + use_heading_from_path: false + approach_velocity_scaling_dist: 1.0 + +Acknowledgements +**************** + +We acknowledge the contributions of: + +1. The author of `Vector Pursuit Path Tracking for Autonomous Ground Vehicles `_, Jeffrey S. Wіt. +2. The `Nav2 Regulated Pure Pursuit Controller project `_. diff --git a/plugins/index.rst b/plugins/index.rst index 4c44dd462..9087b8267 100644 --- a/plugins/index.rst +++ b/plugins/index.rst @@ -103,34 +103,39 @@ Costmap Filters Controllers =========== -+--------------------------------+--------------------+----------------------------------+-----------------------+ -| Plugin Name | Creator | Description | Drivetrain support | -+================================+====================+==================================+=======================+ -| `DWB Controller`_ | David Lu!! | A highly configurable DWA | Differential, | -| | | implementation with plugin | Omnidirectional, | -| | | interfaces | Legged | -+--------------------------------+--------------------+----------------------------------+-----------------------+ -| `TEB Controller`_ | Christoph Rösmann | A MPC-like controller suitable | **Ackermann**, Legged,| -| | | for ackermann, differential, and | Omnidirectional, | -| | | holonomic robots. | Differential | -+--------------------------------+--------------------+----------------------------------+-----------------------+ -| `Regulated Pure Pursuit`_ | Steve Macenski | A service / industrial robot | **Ackermann**, Legged,| -| | | variation on the pure pursuit | Differential | -| | | algorithm with adaptive features.| | -+--------------------------------+--------------------+----------------------------------+-----------------------+ -| `MPPI Controller`_ | Steve Macenski | A predictive MPC controller with | Differential, Omni, | -| | Aleksei Budyakov | modular & custom cost functions | **Ackermann** | -| | | that can accomplish many tasks. | | -+--------------------------------+--------------------+----------------------------------+-----------------------+ -| `Rotation Shim Controller`_ | Steve Macenski | A "shim" controller to rotate | Differential, Omni, | -| | | to path heading before passing | model rotate in place | -| | | to main controller for tracking.| | -+--------------------------------+--------------------+----------------------------------+-----------------------+ -| `Graceful Controller`_ | Alberto Tudela | A controller based on a | Differential | -| | | pose-following control law to | | -| | | generate smooth trajectories. | | -+--------------------------------+--------------------+----------------------------------+-----------------------+ - ++--------------------------------+----------------------+----------------------------------+-----------------------+ +| Plugin Name | Creator | Description | Drivetrain support | ++================================+======================+==================================+=======================+ +| `Vector Pursuit Controller`_ | Black Coffee Robotics| A controller based on the vector | Differential, | +| | | pursuit algorithm useful for | Ackermann, Legged, | +| | | high speed accurate path tracking| | ++--------------------------------+----------------------+----------------------------------+-----------------------+ +| `DWB Controller`_ | David Lu!! | A highly configurable DWA | Differential, | +| | | implementation with plugin | Omnidirectional, | +| | | interfaces | Legged | ++--------------------------------+----------------------+----------------------------------+-----------------------+ +| `TEB Controller`_ | Christoph Rösmann | A MPC-like controller suitable | **Ackermann**, Legged,| +| | | for ackermann, differential, and | Omnidirectional, | +| | | holonomic robots. | Differential | ++--------------------------------+----------------------+----------------------------------+-----------------------+ +| `Regulated Pure Pursuit`_ | Steve Macenski | A service / industrial robot | **Ackermann**, Legged,| +| | | variation on the pure pursuit | Differential | +| | | algorithm with adaptive features.| | ++--------------------------------+----------------------+----------------------------------+-----------------------+ +| `MPPI Controller`_ | Steve Macenski | A predictive MPC controller with | Differential, Omni, | +| | Aleksei Budyakov | modular & custom cost functions | **Ackermann** | +| | | that can accomplish many tasks. | | ++--------------------------------+----------------------+----------------------------------+-----------------------+ +| `Rotation Shim Controller`_ | Steve Macenski | A "shim" controller to rotate | Differential, Omni, | +| | | to path heading before passing | model rotate in place | +| | | to main controller for tracking.| | ++--------------------------------+----------------------+----------------------------------+-----------------------+ +| `Graceful Controller`_ | Alberto Tudela | A controller based on a | Differential | +| | | pose-following control law to | | +| | | generate smooth trajectories. | | ++--------------------------------+----------------------+----------------------------------+-----------------------+ + +.. _Vector Pursuit Controller: https://github.com/blackcoffeerobotics/vector_pursuit_controller .. _DWB Controller: https://github.com/ros-planning/navigation2/tree/main/nav2_dwb_controller .. _TEB Controller: https://github.com/rst-tu-dortmund/teb_local_planner .. _Regulated Pure Pursuit: https://github.com/ros-planning/navigation2/tree/main/nav2_regulated_pure_pursuit_controller diff --git a/setup_guides/algorithm/select_algorithm.rst b/setup_guides/algorithm/select_algorithm.rst index 808947cb2..80979cf40 100644 --- a/setup_guides/algorithm/select_algorithm.rst +++ b/setup_guides/algorithm/select_algorithm.rst @@ -77,6 +77,8 @@ The default controller plugin is the `DWB controller `_ . It implements the `Vector Pursuit algorithm `_ and calculates the command velocity using screw theory. This controller is suitable for high speed path tracking and sharp turns or when computation resources are limited. It can be used for **differential, ackermann, and legged** robots. + Another example of a controller server plugin is the `TEB controller `_ which is an MPC time optimal controller. It implements the Timed Elastic Band (TEB) approach which optimizes the robot's trajectory based on its execution time, distance from obstacles, and feasibility with respect to the robot's kinematic constraints. This controller can be used on **differential, omnidirectional, ackermann, and legged** robots. The last example for this section is the `Regulated Pure Pursuit controller (RPP) `_ . This controller implements a variant of the pure pursuit algorithm with added regulation heuristic functions to manage collision and velocity constraints. This variation is implemented to target the needs of service or industrial robots and is suitable for use with **differential, ackermann, and legged** robots. @@ -87,6 +89,8 @@ Summary +-----------------+---------------------------------------------------+----------------------------+ | Plugin Name | Supported Robot Types | Task | +=================+===================================================+============================+ +| VP controller | Differential, Ackermann, Legged | High speed path tracking | ++-----------------+---------------------------------------------------+----------------------------+ | DWB controller | Differential, Omnidirectional | Dynamic obstacle avoidance | +-----------------+---------------------------------------------------+ | | TEB Controller | Differential, Omnidirectional, Ackermann, Legged | | diff --git a/tuning/index.rst b/tuning/index.rst index 77e10b562..a55906eeb 100644 --- a/tuning/index.rst +++ b/tuning/index.rst @@ -86,6 +86,8 @@ In general though, the following table is a good first-order description of the +----------------+---------------------------------------------------+----------------------------+ | Plugin Name | Supported Robot Types | Task | +================+===================================================+============================+ +| VP controller | Differential, Ackermann, Legged | High speed path tracking | ++----------------+---------------------------------------------------+----------------------------+ | DWB controller | Differential, Omnidirectional | Dynamic obstacle avoidance | +----------------+---------------------------------------------------+ | | MPPI Controller| Differential, Omnidirectional, Ackermann, Legged | Dynamic obstacle avoidance | @@ -99,6 +101,8 @@ All of the above controllers can handle both circular and arbitrary shaped robot Regulated Pure Pursuit is good for exact path following and is typically paired with one of the kinematically feasible planners (eg State Lattice, Hybrid-A\*, etc) since those paths are known to be drivable given hard physical constraints. However, it can also be applied to differential drive robots who can easily pivot to match any holonomic path. This is the plugin of choice if you simply want your robot to follow the path, rather exactly, without any dynamic obstacle avoidance or deviation. It is simple and geometric, as well as slowing the robot in the presence of near-by obstacles *and* while making sharp turns. +Vector Pursuit is another good path tracking solution and just like RPP, is paired with a kinematically feasible planner. It is a bit more advanced than RPP in the sense it also takes path heading into account and can handle more complex paths at higher speeds, but it is still a simple geometric controller thus requiring low computation resources. + DWB and MPPI are both options that will track paths, but also diverge from the path if there are dynamic obstacles present (in order to avoid them). DWB does this through scoring multiple trajectories on a set of critics. These trajectories are also generated via plugins that can be replaced, but support out of the box Omni and Diff robot types within the valid velocity and acceleration restrictions. These critics are plugins that can be selected at run-time and contain weights that may be tuned to create the desired behavior, such as minimizing path distance, minimizing distance to the goal or headings, and other action penalties that can be designed. This does require a bit of tuning for a given platform, application, and desired behavior, but it is possible to tune DWB to do nearly any single thing well. MPPI on the other hand implements an optimization based approach, using randomly perturbed samples of the previous optimal trajectory to maximize a set of plugin-based objective functions. In that regard, it is similar to DWB however MPPI is a far more modern and advanced technique that will deal with dynamic agents in the environment and create intelligent behavior due to the optimization based trajectory planning, rather then DWB's constant action model. MPPI however does have moderately higher compute costs, but it is highly recommended to go this route and has received considerable development resources and attention due to its power. This typically works pretty well out of the box, but to tune for specific behaviors, you may have to retune some of the parameters. The README.md file for this package contains details on how to tune it efficiently. From 9cf7233967c64cb985ed12a647c6542f942765cf Mon Sep 17 00:00:00 2001 From: Kostubh Khandelwal Date: Mon, 7 Oct 2024 18:18:06 +0000 Subject: [PATCH 2/3] Removed vp config file and fixed formatting Signed-off-by: Kostubh Khandelwal --- configuration/index.rst | 1 - .../configuring-vector-pursuit-controller.rst | 394 ------------------ plugins/index.rst | 62 +-- 3 files changed, 31 insertions(+), 426 deletions(-) delete mode 100644 configuration/packages/configuring-vector-pursuit-controller.rst diff --git a/configuration/index.rst b/configuration/index.rst index ba9f1767e..aaff8e8ad 100644 --- a/configuration/index.rst +++ b/configuration/index.rst @@ -21,7 +21,6 @@ the best navigation performance. packages/configuring-controller-server.rst packages/configuring-dwb-controller.rst packages/configuring-regulated-pp.rst - packages/configuring-vector-pursuit-controller.rst packages/configuring-mppic.rst packages/configuring-rotation-shim-controller.rst packages/configuring-graceful-motion-controller.rst diff --git a/configuration/packages/configuring-vector-pursuit-controller.rst b/configuration/packages/configuring-vector-pursuit-controller.rst deleted file mode 100644 index 2fd2f2bdb..000000000 --- a/configuration/packages/configuring-vector-pursuit-controller.rst +++ /dev/null @@ -1,394 +0,0 @@ -.. _configuring_vector_pursuit_controller: - -Vector Pursuit Controller -######################### - -Source code on Github_. - -.. _Github: https://github.com/blackcoffeerobotics/vector_pursuit_controller - -The Vector Pursuit controller implements a path-tracking algorithm based on the theory of screws. Unlike most other controllers, Vector Pursuit incorporates the path's orientation with the position when calculating velocity commands. -The use of screws unifies rotation and translation, making the controller more robust and geometrically meaningful, especially in complex, dynamic environments. This enables the robot to follow the path more smoothly and accurately, especially when navigating tight corners or moving at high speeds. -By adjusting linear velocities based on path curvature, the controller minimizes overshoot in sharp turns, enhancing safety and stability. - -See the package's ``README`` for more complete information. - - -Vector Pursuit Parameters -********************************* - -:k: - - ============== =========================== - Type Default - -------------- --------------------------- - double 8.0 - ============== =========================== - - Description - k is a constant that relates the rotation and translation times when calculating the resultant screw(as a sum of the rotation and translation screws). The relationship is rotation_time = k * translation_time. Increasing k will put more weightage on faster translation and decreasing k will, in turn, apply more weightage on faster rotation. - -:desired_linear_vel: - - ============== =========================== - Type Default - -------------- --------------------------- - double 0.5 - ============== =========================== - - Description - The desired maximum linear velocity (m/s). -:min_turning_radius: - - ============== =========================== - Type Default - -------------- --------------------------- - double 1.0 - ============== =========================== - - Description - The minimum achievable turning radius(m) of the robot. Remember, sharper turns have smaller radii. - -:lookahead_dist: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 0.6 - ============== ============================= - - Description - The lookahead distance (m) to find the lookahead point. - -:min_lookahead_dist: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 0.3 - ============== ============================= - - Description - The minimum lookahead distance (m) threshold for adaptive lookahead point. - -:max_lookahead_dist: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 0.9 - ============== ============================= - - Description - The maximum lookahead distance (m) threshold for adaptive lookahead point. - -:lookahead_time: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 1.5 - ============== ============================= - - Description - The time (s) to integrate the current linear velocity to get the scaled lookahead distance for adaptive lookahead point. - -:rotate_to_heading_angular_vel: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 1.8 - ============== ============================= - - Description - Angular velocity for rotating to heading. - -:transform_tolerance: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 0.1 - ============== ============================= - - Description - The TF transform tolerance (s). - -:use_velocity_scaled_lookahead_dist: - - ============== ============================= - Type Default - -------------- ----------------------------- - bool false - ============== ============================= - - Description - Enable velocity adaptive ``lookahead_distance``. - -:min_approach_linear_velocity: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 0.05 - ============== ============================= - - Description - The minimum velocity approach aware linear velocity scaling can produce. - -:approach_velocity_scaling_dist: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 1.0 - ============== ============================= - - Description - The distance to goal at which velocity scaling will begin. Set to 0 to disable. - -:use_collision_detection: - - ============== ============================= - Type Default - -------------- ----------------------------- - bool true - ============== ============================= - - Description - Enable/disable collision detection. - -:max_allowed_time_to_collision_up_to_target: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 1.0 - ============== ============================= - - Description - Maximum time (s) allowed for collision checking. - -:use_cost_regulated_linear_velocity_scaling: - - ============== ============================= - Type Default - -------------- ----------------------------- - bool true - ============== ============================= - - Description - Whether to slowdown in close proximity to obstacles. - -:cost_scaling_dist: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 0.6 - ============== ============================= - - Description - Distance for cost-based velocity scaling. - -:cost_scaling_gain: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 1.0 - ============== ============================= - - Description - Multiplicative factor for cost-based velocity scaling. - -:inflation_cost_scaling_factor: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 3.0 - ============== ============================= - - Description - Factor for inflation cost scaling. - -:min_linear_velocity: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 0.05 - ============== ============================= - - Description - The minimum speed (m/s) robot must run at. Must be ``> 0.01``. - -:use_rotate_to_heading: - - ============== ============================= - Type Default - -------------- ----------------------------- - bool true - ============== ============================= - - Description - Enable/disable rotate-to-heading behavior. Will override reversing if both are enabled. - -:allow_reversing: - - ============== ============================= - Type Default - -------------- ----------------------------- - bool false - ============== ============================= - - Description - Enable/Disable reversing movement. Will move in reverse if the lookahead point is behind the robot. - -:rotate_to_heading_min_angle: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 0.785 - ============== ============================= - - Description - The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place, if ``use_rotate_to_heading`` is ``true``. - -:max_angular_accel: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 3.2 - ============== ============================= - - Description - Maximum allowable angular acceleration (rad/s^2). - -:max_linear_accel: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 2.0 - ============== ============================= - - Description - Maximum allowable linear acceleration (m/s^2). - -:max_lateral_accel: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 0.5 - ============== ============================= - - Description - Maximum allowable acceleration (m/s^2) perpendicular to axis of desired translation motion. This limit is used to restrict the maximum speed for a given path curvature. - -:use_interpolation: - - ============== ============================= - Type Default - -------------- ----------------------------- - bool true - ============== ============================= - - Description - Calculate lookahead point exactly at the lookahead distance. Otherwise select a discrete point on the path. - -:use_heading_from_path: - - ============== ============================= - Type Default - -------------- ----------------------------- - bool false - ============== ============================= - - Description - If set to true, uses the orientation from the path poses otherwise compute appropriate orientations. Only set to true if using a planner that takes robot heading into account like Smac Planner. - -:max_robot_pose_search_dist: - - ============== ================================================= - Type Default - -------------- ------------------------------------------------- - double Local costmap max extent (max(width, height) / 2) - ============== ================================================= - - Description - Maximum search distance for target poses along the global plan. - -Example -******* -.. code-block:: yaml - - controller_server: - ros__parameters: - use_sim_time: True - controller_frequency: 20.0 - min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.5 - min_theta_velocity_threshold: 0.001 - failure_tolerance: 0.3 - progress_checker_plugin: "progress_checker" - goal_checker_plugins: ["general_goal_checker"] - controller_plugins: ["FollowPath"] - - # Progress checker parameters - progress_checker: - plugin: "nav2_controller::SimpleProgressChecker" - required_movement_radius: 0.25 - movement_time_allowance: 10.0 - - # Goal checker parameters - general_goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.25 - yaw_goal_tolerance: 0.25 - stateful: True - - FollowPath: - plugin: "vector_pursuit_controller::VectorPursuitController" - k: 5.0 - desired_linear_vel: 0.5 - min_turning_radius: 0.25 - lookahead_dist: 1.0 - min_lookahead_dist: 0.5 - max_lookahead_dist: 1.5 - lookahead_time: 1.5 - rotate_to_heading_angular_vel: 0.5 - transform_tolerance: 0.1 - use_velocity_scaled_lookahead_dist: false - min_linear_velocity: 0.0 - min_approach_linear_velocity: 0.05 - approach_velocity_scaling_dist: 0.5 - max_allowed_time_to_collision_up_to_target: 1.0 - use_collision_detection: true - use_cost_regulated_linear_velocity_scaling: true - cost_scaling_dist: 0.5 - cost_scaling_gain: 1.0 - inflation_cost_scaling_factor: 3.0 - use_rotate_to_heading: true - allow_reversing: false - rotate_to_heading_min_angle: 0.5 - max_angular_accel: 3.0 - max_linear_accel: 2.0 - max_lateral_accel: 0.2 - max_robot_pose_search_dist: 10.0 - use_interpolation: true - use_heading_from_path: false - approach_velocity_scaling_dist: 1.0 - -Acknowledgements -**************** - -We acknowledge the contributions of: - -1. The author of `Vector Pursuit Path Tracking for Autonomous Ground Vehicles `_, Jeffrey S. Wіt. -2. The `Nav2 Regulated Pure Pursuit Controller project `_. diff --git a/plugins/index.rst b/plugins/index.rst index 9087b8267..a4cc31669 100644 --- a/plugins/index.rst +++ b/plugins/index.rst @@ -103,37 +103,37 @@ Costmap Filters Controllers =========== -+--------------------------------+----------------------+----------------------------------+-----------------------+ -| Plugin Name | Creator | Description | Drivetrain support | -+================================+======================+==================================+=======================+ -| `Vector Pursuit Controller`_ | Black Coffee Robotics| A controller based on the vector | Differential, | -| | | pursuit algorithm useful for | Ackermann, Legged, | -| | | high speed accurate path tracking| | -+--------------------------------+----------------------+----------------------------------+-----------------------+ -| `DWB Controller`_ | David Lu!! | A highly configurable DWA | Differential, | -| | | implementation with plugin | Omnidirectional, | -| | | interfaces | Legged | -+--------------------------------+----------------------+----------------------------------+-----------------------+ -| `TEB Controller`_ | Christoph Rösmann | A MPC-like controller suitable | **Ackermann**, Legged,| -| | | for ackermann, differential, and | Omnidirectional, | -| | | holonomic robots. | Differential | -+--------------------------------+----------------------+----------------------------------+-----------------------+ -| `Regulated Pure Pursuit`_ | Steve Macenski | A service / industrial robot | **Ackermann**, Legged,| -| | | variation on the pure pursuit | Differential | -| | | algorithm with adaptive features.| | -+--------------------------------+----------------------+----------------------------------+-----------------------+ -| `MPPI Controller`_ | Steve Macenski | A predictive MPC controller with | Differential, Omni, | -| | Aleksei Budyakov | modular & custom cost functions | **Ackermann** | -| | | that can accomplish many tasks. | | -+--------------------------------+----------------------+----------------------------------+-----------------------+ -| `Rotation Shim Controller`_ | Steve Macenski | A "shim" controller to rotate | Differential, Omni, | -| | | to path heading before passing | model rotate in place | -| | | to main controller for tracking.| | -+--------------------------------+----------------------+----------------------------------+-----------------------+ -| `Graceful Controller`_ | Alberto Tudela | A controller based on a | Differential | -| | | pose-following control law to | | -| | | generate smooth trajectories. | | -+--------------------------------+----------------------+----------------------------------+-----------------------+ ++--------------------------------+-----------------------+------------------------------------+-----------------------+ +| Plugin Name | Creator | Description | Drivetrain support | ++================================+=======================+====================================+=======================+ +| `DWB Controller`_ | David Lu!! | A highly configurable DWA | Differential, | +| | | implementation with plugin | Omnidirectional, | +| | | interfaces | Legged | ++--------------------------------+-----------------------+------------------------------------+-----------------------+ +| `TEB Controller`_ | Christoph Rösmann | A MPC-like controller suitable | **Ackermann**, Legged,| +| | | for ackermann, differential, and | Omnidirectional, | +| | | holonomic robots. | Differential | ++--------------------------------+-----------------------+------------------------------------+-----------------------+ +| `Regulated Pure Pursuit`_ | Steve Macenski | A service / industrial robot | **Ackermann**, Legged,| +| | | variation on the pure pursuit | Differential | +| | | algorithm with adaptive features. | | ++--------------------------------+-----------------------+------------------------------------+-----------------------+ +| `MPPI Controller`_ | Steve Macenski | A predictive MPC controller with | Differential, Omni, | +| | Aleksei Budyakov | modular & custom cost functions | **Ackermann** | +| | | that can accomplish many tasks. | | ++--------------------------------+-----------------------+------------------------------------+-----------------------+ +| `Rotation Shim Controller`_ | Steve Macenski | A "shim" controller to rotate | Differential, Omni, | +| | | to path heading before passing | model rotate in place | +| | | to main controller for tracking. | | ++--------------------------------+-----------------------+------------------------------------+-----------------------+ +| `Graceful Controller`_ | Alberto Tudela | A controller based on a | Differential | +| | | pose-following control law to | | +| | | generate smooth trajectories. | | ++--------------------------------+-----------------------+------------------------------------+-----------------------+ +| `Vector Pursuit Controller`_ | Black Coffee Robotics | A controller based on the vector | Differential, | +| | | pursuit algorithm useful for | Ackermann, Legged, | +| | | high speed accurate path tracking. | | ++--------------------------------+-----------------------+------------------------------------+-----------------------+ .. _Vector Pursuit Controller: https://github.com/blackcoffeerobotics/vector_pursuit_controller .. _DWB Controller: https://github.com/ros-planning/navigation2/tree/main/nav2_dwb_controller From 8a8b53e01b0ddd2cdfce415d0d41fbb3740a0023 Mon Sep 17 00:00:00 2001 From: Kostubh Khandelwal Date: Tue, 8 Oct 2024 15:35:01 +0000 Subject: [PATCH 3/3] Description refactoring and reordering Signed-off-by: Kostubh Khandelwal --- setup_guides/algorithm/select_algorithm.rst | 10 +++++----- tuning/index.rst | 10 +++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/setup_guides/algorithm/select_algorithm.rst b/setup_guides/algorithm/select_algorithm.rst index 80979cf40..2874d1026 100644 --- a/setup_guides/algorithm/select_algorithm.rst +++ b/setup_guides/algorithm/select_algorithm.rst @@ -77,11 +77,11 @@ The default controller plugin is the `DWB controller `_ . It implements the `Vector Pursuit algorithm `_ and calculates the command velocity using screw theory. This controller is suitable for high speed path tracking and sharp turns or when computation resources are limited. It can be used for **differential, ackermann, and legged** robots. +Next example of a controller server plugin is the `TEB controller `_ which is an MPC time optimal controller. It implements the Timed Elastic Band (TEB) approach which optimizes the robot's trajectory based on its execution time, distance from obstacles, and feasibility with respect to the robot's kinematic constraints. This controller can be used on **differential, omnidirectional, ackermann, and legged** robots. -Another example of a controller server plugin is the `TEB controller `_ which is an MPC time optimal controller. It implements the Timed Elastic Band (TEB) approach which optimizes the robot's trajectory based on its execution time, distance from obstacles, and feasibility with respect to the robot's kinematic constraints. This controller can be used on **differential, omnidirectional, ackermann, and legged** robots. +Another example for this section is the `Regulated Pure Pursuit controller (RPP) `_ . This controller implements a variant of the pure pursuit algorithm with added regulation heuristic functions to manage collision and velocity constraints. This variation is implemented to target the needs of service or industrial robots and is suitable for use with **differential, ackermann, and legged** robots. -The last example for this section is the `Regulated Pure Pursuit controller (RPP) `_ . This controller implements a variant of the pure pursuit algorithm with added regulation heuristic functions to manage collision and velocity constraints. This variation is implemented to target the needs of service or industrial robots and is suitable for use with **differential, ackermann, and legged** robots. +The last example is the `Vector Pursuit Controller `_ . It implements the `Vector Pursuit algorithm `_ and calculates the command velocity using screw theory. This controller is suitable for high speed path tracking and sharp turns or when computation resources are limited. It can be used for **differential, ackermann, and legged** robots. Summary ------- @@ -89,8 +89,6 @@ Summary +-----------------+---------------------------------------------------+----------------------------+ | Plugin Name | Supported Robot Types | Task | +=================+===================================================+============================+ -| VP controller | Differential, Ackermann, Legged | High speed path tracking | -+-----------------+---------------------------------------------------+----------------------------+ | DWB controller | Differential, Omnidirectional | Dynamic obstacle avoidance | +-----------------+---------------------------------------------------+ | | TEB Controller | Differential, Omnidirectional, Ackermann, Legged | | @@ -99,6 +97,8 @@ Summary +-----------------+---------------------------------------------------+----------------------------+ | MPPI controller | Differential, Ackermann, Legged, Omnidirectional | Modern MPC controller | +-----------------+---------------------------------------------------+----------------------------+ +| VP controller | Differential, Ackermann, Legged | High speed path tracking | ++-----------------+---------------------------------------------------+----------------------------+ All of these algorithms work for both circular and non-circular robots. diff --git a/tuning/index.rst b/tuning/index.rst index a55906eeb..5e9495e89 100644 --- a/tuning/index.rst +++ b/tuning/index.rst @@ -86,8 +86,6 @@ In general though, the following table is a good first-order description of the +----------------+---------------------------------------------------+----------------------------+ | Plugin Name | Supported Robot Types | Task | +================+===================================================+============================+ -| VP controller | Differential, Ackermann, Legged | High speed path tracking | -+----------------+---------------------------------------------------+----------------------------+ | DWB controller | Differential, Omnidirectional | Dynamic obstacle avoidance | +----------------+---------------------------------------------------+ | | MPPI Controller| Differential, Omnidirectional, Ackermann, Legged | Dynamic obstacle avoidance | @@ -96,18 +94,20 @@ In general though, the following table is a good first-order description of the +----------------+---------------------------------------------------+----------------------------+ | Rotation Shim | Differential, Omnidirectional | Rotate to rough heading | +----------------+---------------------------------------------------+----------------------------+ +| VP controller | Differential, Ackermann, Legged | High speed path tracking | ++----------------+---------------------------------------------------+----------------------------+ All of the above controllers can handle both circular and arbitrary shaped robots in configuration. Regulated Pure Pursuit is good for exact path following and is typically paired with one of the kinematically feasible planners (eg State Lattice, Hybrid-A\*, etc) since those paths are known to be drivable given hard physical constraints. However, it can also be applied to differential drive robots who can easily pivot to match any holonomic path. This is the plugin of choice if you simply want your robot to follow the path, rather exactly, without any dynamic obstacle avoidance or deviation. It is simple and geometric, as well as slowing the robot in the presence of near-by obstacles *and* while making sharp turns. -Vector Pursuit is another good path tracking solution and just like RPP, is paired with a kinematically feasible planner. It is a bit more advanced than RPP in the sense it also takes path heading into account and can handle more complex paths at higher speeds, but it is still a simple geometric controller thus requiring low computation resources. - DWB and MPPI are both options that will track paths, but also diverge from the path if there are dynamic obstacles present (in order to avoid them). DWB does this through scoring multiple trajectories on a set of critics. These trajectories are also generated via plugins that can be replaced, but support out of the box Omni and Diff robot types within the valid velocity and acceleration restrictions. These critics are plugins that can be selected at run-time and contain weights that may be tuned to create the desired behavior, such as minimizing path distance, minimizing distance to the goal or headings, and other action penalties that can be designed. This does require a bit of tuning for a given platform, application, and desired behavior, but it is possible to tune DWB to do nearly any single thing well. MPPI on the other hand implements an optimization based approach, using randomly perturbed samples of the previous optimal trajectory to maximize a set of plugin-based objective functions. In that regard, it is similar to DWB however MPPI is a far more modern and advanced technique that will deal with dynamic agents in the environment and create intelligent behavior due to the optimization based trajectory planning, rather then DWB's constant action model. MPPI however does have moderately higher compute costs, but it is highly recommended to go this route and has received considerable development resources and attention due to its power. This typically works pretty well out of the box, but to tune for specific behaviors, you may have to retune some of the parameters. The README.md file for this package contains details on how to tune it efficiently. -Finally, the Rotation Shim Plugin helps assist plugins like TEB and DWB (among others) to rotate the robot in place towards a new path's heading before starting to track the path. This allows you to tune your local trajectory planner to operate with a desired behavior without having to worry about being able to rotate on a dime with a significant deviation in angular distance over a very small euclidean distance. Some controllers when heavily tuned for accurate path tracking are constrained in their actions and don't very cleanly rotate to a new heading. Other controllers have a 'spiral out' behavior because their sampling requires some translational velocity, preventing it from simply rotating in place. This helps alleviate that problem and makes the robot rotate in place very smoothly. +The Rotation Shim Plugin helps assist plugins like TEB and DWB (among others) to rotate the robot in place towards a new path's heading before starting to track the path. This allows you to tune your local trajectory planner to operate with a desired behavior without having to worry about being able to rotate on a dime with a significant deviation in angular distance over a very small euclidean distance. Some controllers when heavily tuned for accurate path tracking are constrained in their actions and don't very cleanly rotate to a new heading. Other controllers have a 'spiral out' behavior because their sampling requires some translational velocity, preventing it from simply rotating in place. This helps alleviate that problem and makes the robot rotate in place very smoothly. + +Finally, Vector Pursuit is another good path tracking solution and just like RPP, is paired with a kinematically feasible planner. It is a bit more advanced than RPP in the sense it also takes path heading into account. Vector Pursuit can handle complex paths at high speeds, but it is still a simple geometric controller thus requiring low computation resources. .. note:: These are simply the default and available plugins from the community. For a specific robot platform / company, you may also choose to use none of these and create your own. See the :ref:`writing_new_nav2controller_plugin` tutorial for more details. If you're willing to contribute this work back to the community, please file a ticket or contact a maintainer! They'd love to hear from you.