adding MPPI align params for alignment

configuration/packages/configuring-mppic.rst

@@ -289,6 +289,38 @@ Path Handler
   Description
     Max integrated distance ahead of robot pose to search for nearest path point in case of path looping.
 
+:enforce_path_inversion:
+
+  ============== ===========================
+  Type           Default                    
+  -------------- ---------------------------
+  bool           false
+  ============== ===========================
+
+  Description
+    If true, it will prune paths containing cusping points for segments changing directions (e.g. path inversions) such that the controller will be forced to change directions at or very near the planner's requested inversion point. This is targeting Smac Planner users with feasible paths who need their robots to switch directions where specifically requested.
+
+:inversion_xy_tolerance:
+
+  ============== ===========================
+  Type           Default                    
+  -------------- ---------------------------
+  double         0.2
+  ============== ===========================
+
+  Description
+    Cartesian proximity (m) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion.
+
+:inversion_yaw_tolerance:
+
+  ============== ===========================
+  Type           Default                    
+  -------------- ---------------------------
+  double         0.4
+  ============== ===========================
+
+  Description
+    Angular proximity (radians) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. 0.4 rad = 23 deg.
 
 Ackermann Motion Model
 ----------------------
@@ -562,6 +594,17 @@ Path Align Critic
   Description
     Maximum proportion of the path that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presence of dynamic objects in the scene. Between 0-1 for 0-100%.
 
+:use_path_orientations:
+
+  ============== ===========================
+  Type           Default                    
+  -------------- ---------------------------
+  bool           false
+  ============== ===========================
+
+  Description
+    Whether to consider path's orientations in path alignment, which can be useful when paired with feasible smac planners to incentivize directional changes only where/when the smac planner requests them. If you want the robot to deviate and invert directions where the controller sees fit, keep as false. If your plans do not contain orientation information (e.g. navfn), keep as false.
+
 Path Angle Critic
 -----------------
 
@@ -811,6 +854,7 @@ Example
             trajectory_point_step: 3
             threshold_to_consider: 0.5
             offset_from_furthest: 20
+            use_path_orientations: false
           PathFollowCritic:
             enabled: true
             cost_power: 1

migration/Iron.rst

@@ -23,3 +23,11 @@ MPPI's Path Angle critic now has a ``mode`` setting to adjust behavior depending
 ``mode: 1`` sets the penalization of path's relative directions by either forward orientation or the opposite for reversing to allow for true bidirectional motion when one way or another is not preferable for a symmetric robot. This uses only the path's relative points to the robot to decide which direction to incentivize. 
 
 ``mode: 2`` instead uses the path's orientations when a feasible path is given from the Smac Planners or the Smoother server's algorithms. This way, the globally planned orientations are followed rather than the based solely on the path's relative points. This is useful for non-circular robots in highly confined settings where there may be restricted opportunities to change directions so following the global path's orientation are required to end in the orientation you require.
+
+
+Changes to MPPI Path Handling For Directionality
+************************************************
+
+MPPI's Path Align Critic and Path Handler object now have options to utilize the path's orientation information to force the controller to change directions when and only when requested by a feasible planner. When ``enforce_path_inversion`` is ``true``, the path handler will prune the path to the first time the directions change to force the controller to plan to the inversion point and then be set the rest of the path, once in tolerance. The Path Align critic also contains a parameter ``use_path_orientations``  which can be paired with it to incentivize aligning the path containing orientation information to better attempt to achieve path inversions where requested **and** not do them when not requested.
+
+See MPPI's configuration guide for complete information.