Added Vector Pursuit Controller (ros-navigation#596)

* Added Vector Pursuit Controller Signed-off-by: Kostubh Khandelwal <[email protected]> * Removed vp config file and fixed formatting Signed-off-by: Kostubh Khandelwal <[email protected]> * Description refactoring and reordering Signed-off-by: Kostubh Khandelwal <[email protected]> --------- Signed-off-by: Kostubh Khandelwal <[email protected]> Signed-off-by: Rob Fisher <[email protected]>
robbiefish · Nov 14, 2024 · 9c80ac7 · 9c80ac7
1 parent 2916a10
commit 9c80ac7
Show file tree

Hide file tree

Showing 3 changed files with 44 additions and 31 deletions.
diff --git 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    |
++================================+=======================+====================================+=======================+
+|  `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
 .. _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
@@ -77,9 +77,11 @@ The default controller plugin is the `DWB controller <https://docs.nav2.org/conf
 
 The ``DWB controller`` can be used on **circular or non-circular differential, and circular or non-circular omnidirectional** robots. It may also be configured for **ackerman and legged** robots if it is given a ``Trajectory Generation plugin`` that produces a set of possible trajectories that considers the robot's minimum curvature constraint.
 
-Another example of a controller server plugin is the `TEB controller <https://github.com/rst-tu-dortmund/teb_local_planner>`_ 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.
+Next example of a controller server plugin is the `TEB controller <https://github.com/rst-tu-dortmund/teb_local_planner>`_ 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) <https://docs.nav2.org/configuration/packages/configuring-regulated-pp.html>`_ . 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.
+Another example for this section is the `Regulated Pure Pursuit controller (RPP) <https://docs.nav2.org/configuration/packages/configuring-regulated-pp.html>`_ . 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 <https://github.com/blackcoffeerobotics/vector_pursuit_controller>`_ . It implements the `Vector Pursuit algorithm <https://apps.dtic.mil/sti/pdfs/ADA468928.pdf>`_ 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
 -------
@@ -95,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
@@ -94,6 +94,8 @@ 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.
 
@@ -103,7 +105,9 @@ DWB and MPPI are both options that will track paths, but also diverge from the p
 
 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.