update docs for iterative graceful controller

configuration/packages/configuring-graceful-motion-controller.rst

@@ -25,16 +25,27 @@ Graceful Controller Parameters
   Description
     The TF transform tolerance (s).
 
-:motion_target_dist:
+:max_lookahead:
 
   ============== =============================
   Type           Default                      
   -------------- -----------------------------
-  double         0.6
+  double         1.0
   ============== =============================
 
   Description
-    The lookahead distance (m) to use to find the motion_target point.
+    The maximum lookahead distance (m) to use when selecting a target pose for the underlying control law. Using poses that are further away will generally result in smoother operations, but simulating poses that are very far away can result in reduced performance, especially in tight or cluttered environments. If the controller cannot forward simulate to a pose this far away without colliding, it will iteratively select a target pose that is closer to the robot.
+
+:min_lookahead:
+
+  ============== =============================
+  Type           Default                      
+  -------------- -----------------------------
+  double         0.25
+  ============== =============================
+
+  Description
+    The minimum lookahead distance (m) to use when selecting a target pose for the underlying control law. This parameter avoids instability when an unexpected obstacle appears in the path of the robot by returning failure, which typically triggers replanning.
 
 :max_robot_pose_search_dist:
 
@@ -52,29 +63,29 @@ Graceful Controller Parameters
   ============== =============================
   Type           Default                      
   -------------- -----------------------------
-  double         3.0 
+  double         2.0
   ============== =============================
 
   Description
-    Ratio of the rate of change in phi to the rate of change in r. Controls the convergence of the slow subsystem. If this value is equal to zero, the controller will behave as a pure waypoint follower. A high value offers extreme scenario of pose-following where theta is reduced much faster than r.
+    Ratio of the rate of change in phi to the rate of change in r. Controls the convergence of the slow subsystem. If this value is equal to zero, the controller will behave as a pure waypoint follower. A high value offers extreme scenario of pose-following where theta is reduced much faster than r. The referenced paper calls this `k1`.
 
 :k_delta:
 
   ============== =============================
   Type           Default                      
   -------------- -----------------------------
-  double         2.0 
+  double         1.0
   ============== =============================
 
   Description
-    Constant factor applied to the heading error feedback. Controls the convergence of the fast subsystem. The bigger the value, the robot converge faster to the reference heading.
+    Constant factor applied to the heading error feedback. Controls the convergence of the fast subsystem. The bigger the value, the robot converge faster to the reference heading. The referenced paper calls this `k2`.
 
 :beta:
 
   ============== =============================
   Type           Default                      
   -------------- -----------------------------
-  double         0.2 
+  double         0.4
   ============== =============================
 
   Description
@@ -124,6 +135,17 @@ Graceful Controller Parameters
   Description
     Maximum angular velocity (rad/s) produced by the control law.
 
+:v_angular_min_in_place:
+
+  ============== =============================
+  Type           Default                      
+  -------------- -----------------------------
+  double         0.25
+  ============== =============================
+
+  Description
+    Minimum angular velocity (rad/s) produced by the control law when rotating in place. This value should be based on the minimum rotation speed controllable by the robot.
+
 :slowdown_radius:
 
   ============== =============================
@@ -146,18 +168,18 @@ Graceful Controller Parameters
   Description
     Enable a rotation in place to the goal before starting the path. The control law may generate large sweeping arcs to the goal pose, depending on the initial robot orientation and ``k_phi``, ``k_delta``.
 
-:initial_rotation_min_angle:
+:initial_rotation_tolerance:
 
   ============== =============================
   Type           Default                      
   -------------- -----------------------------
-  double         0.75 
+  double         0.75
   ============== =============================
 
   Description
-    The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if ``initial_rotation`` is enabled.
+    The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if ``initial_rotation`` is enabled. This value is generally acceptable if continuous replanning is enabled. If not using continuous replanning, a lower value may perform better.
 
-:final_rotation:
+:prefer_final_rotation:
 
   ============== =============================
   Type           Default                      
@@ -166,7 +188,7 @@ Graceful Controller Parameters
   ============== =============================
 
   Description
-    Similar to ``initial_rotation``, the control law can generate large arcs when the goal orientation is not aligned with the path. If this is enabled, the final pose will be ignored and the robot will follow the orientation of he path and will make a final rotation in place to the goal orientation.
+    The control law can generate large arcs when the goal orientation is not aligned with the path. If this is enabled, the orientation of the final pose will be ignored and the robot will follow the orientation of the path and will make a final rotation in place to the goal orientation.
 
 :rotation_scaling_factor:
 
@@ -190,6 +212,17 @@ Graceful Controller Parameters
   Description
     Whether to allow the robot to move backward.
 
+:in_place_collision_tolerance:
+
+  ============== =============================
+  Type           Default                      
+  -------------- -----------------------------
+  double         0.1
+  ============== =============================
+
+  Description
+    When performing an in-place rotation after the XY goal tolerance has been met, this is the angle (in radians) between poses to check for collision.
+
 Example
 *******
 .. code-block:: yaml
@@ -217,10 +250,11 @@ Example
       FollowPath:
         plugin: nav2_graceful_controller::GracefulController
         transform_tolerance: 0.1
-        motion_target_dist: 0.6
+        min_lookahead: 0.25
+        max_lookahead: 1.0
         initial_rotation: true
-        initial_rotation_min_angle: 0.75
-        final_rotation: true
+        initial_rotation_threshold: 0.75
+        prefer_final_rotation: true
         allow_backward: false
         k_phi: 3.0
         k_delta: 2.0
@@ -229,4 +263,5 @@ Example
         v_linear_min: 0.1
         v_linear_max: 1.0
         v_angular_max: 5.0
+        v_angular_min_in_place: 0.25
         slowdown_radius: 1.5

migration/Jazzy.rst

@@ -204,3 +204,13 @@ New Plugin Container Layer
 **************************
 
 In `PR #4781 <https://github.com/ros-navigation/navigation2/pull/4781>`_ a costmap layer plugin type was added to support the grouping of different costmap layers under a single costmap layer. This would allow for different isolated combinations of costmap layers to be combined under one parent costmap instead of the current implementation which would indiscriminately combine all costmap layers together.
+
+Iterative Target Selection for the Graceful Controller
+******************************************************
+
+In `PR #4795 <https://github.com/ros-navigation/navigation2/pull/4795>`_ the ``nav2_graceful_controller`` was updated to iteratively select motion targets. This is a large refactor which significantly improves the performance of the controller. The ``motion_target_dist`` parameter has been replaced by ``min_lookahead`` and ``max_lookahead`` parameters. Additional changes include:
+
+* Improved defaults for ``k_phi``, ``k_delta``, ``beta`` parameters of the underlying control law.
+* Automatic creation of orientations for the plan if they are missing.
+* Addition of ``v_angular_min_in_place`` parameter to avoid the robot getting stuck while rotating due to mechanical limitations.
+* ``final_rotation`` has been renamed ``prefer_final_rotation`` and the behavior has changed slightly.

plugins/index.rst

@@ -133,8 +133,8 @@ Controllers
 |                                |                       | 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      |                       |
+| `Graceful Controller`_         | Alberto Tudela        | A controller based on a            | Differential, Omni,   |
+|                                |                       | pose-following control law to      | Legged                |
 |                                |                       | generate smooth trajectories.      |                       |
 +--------------------------------+-----------------------+------------------------------------+-----------------------+
 | `Vector Pursuit Controller`_   | Black Coffee Robotics | A controller based on the vector   | Differential,         |