adapting goal critic for speed to goal (#3641)

* adapting goal critic for speed to goal

* retuning goal critic

* add readme entries

* Update critics_tests.cpp

nav2_mppi_controller/README.md

@@ -81,14 +81,14 @@ This process is then repeated a number of times and returns a converged solution
  | ---------------                  | ------ | ----------------------------------------------------------------------------------------------------------- |
  | cost_weight                      | double | Default 3.0. Weight to apply to critic term.                                                                |
  | cost_power                       | int    | Default 1. Power order to apply to term.                                                                    |
- | threshold_to_consider            | double | Default 0.40. Minimal distance between robot and goal above which angle goal cost considered.               |
+ | threshold_to_consider            | double | Default 0.5. Minimal distance between robot and goal above which angle goal cost considered.               |
 
 #### Goal Critic
  | Parameter            | Type   | Definition                                                                                                  |
  | -------------------- | ------ | ----------------------------------------------------------------------------------------------------------- |
  | cost_weight          | double | Default 5.0. Weight to apply to critic term.                                                                |
  | cost_power           | int    | Default 1. Power order to apply to term.                                                                    |
- | threshold_to_consider      | double | Default 1.0. Distance between robot and goal above which goal cost starts being considered                                    |
+ | threshold_to_consider      | double | Default 1.4. Distance between robot and goal above which goal cost starts being considered                                    |
 
 
 #### Obstacles Critic
@@ -107,7 +107,7 @@ This process is then repeated a number of times and returns a converged solution
  | ---------------            | ------ | -----------------------------------------------------------------------------------------------------------                        |
  | cost_weight                | double | Default 10.0. Weight to apply to critic term.                                                                                       |
  | cost_power                 | int    | Default 1. Power order to apply to term.                                                                                           |
- | threshold_to_consider      | double | Default 0.4. Distance between robot and goal above which path align cost stops being considered                                    |
+ | threshold_to_consider      | double | Default 0.5. Distance between robot and goal above which path align cost stops being considered                                    |
  | offset_from_furthest      | double | Default 20. Checks that the candidate trajectories are sufficiently far along their way tracking the path to apply the alignment critic. This ensures that path alignment is only considered when actually tracking the path, preventing awkward initialization motions preventing the robot from leaving the path to achieve the appropriate heading.  |
  | trajectory_point_step      | double | Default 4. Step of trajectory points to evaluate for path distance to reduce compute time. Between 1-10 is typically reasonable.   |
  | max_path_occupancy_ratio   | double | Default 0.07 (7%). 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.  |
@@ -117,7 +117,7 @@ This process is then repeated a number of times and returns a converged solution
  | ---------------           | ------ | ----------------------------------------------------------------------------------------------------------- |
  | cost_weight               | double | Default 2.0. Weight to apply to critic term.                                                                |
  | cost_power                | int    | Default 1. Power order to apply to term.                                                                    |
- | threshold_to_consider     | double | Default 0.4. Distance between robot and goal above which path angle cost stops being considered             |
+ | threshold_to_consider     | double | Default 0.5. Distance between robot and goal above which path angle cost stops being considered             |
  | offset_from_furthest      | int    | Default 4. Number of path points after furthest one any trajectory achieves to compute path angle relative to.  |
  | max_angle_to_furthest     | double | Default 1.2. Angular distance between robot and goal above which path angle cost starts being considered           |
  | forward_preference     | bool | Default true. Whether or not your robot has a preference for which way is forward in motion. Different from if reversing is generally allowed, but if you robot contains *no* particular preference one way or another.           |
@@ -129,14 +129,14 @@ This process is then repeated a number of times and returns a converged solution
  | cost_weight           | double | Default 5.0. Weight to apply to critic term.                                                                |
  | cost_power            | int    | Default 1. Power order to apply to term.   |
  | offset_from_furthest  | int    | Default 6. Number of path points after furthest one any trajectory achieves to drive path tracking relative to.     |
- | threshold_to_consider        | float  | Default 0.4. Distance between robot and goal above which path follow cost stops being considered  | 
+ | threshold_to_consider        | float  | Default 1.4. Distance between robot and goal above which path follow cost stops being considered  | 
 
 #### Prefer Forward Critic
  | Parameter             | Type   | Definition                                                                                                  |
  | ---------------       | ------ | ----------------------------------------------------------------------------------------------------------- |
  | cost_weight           | double | Default 5.0. Weight to apply to critic term.                                                                |
  | cost_power            | int    | Default 1. Power order to apply to term.                                                                    |
- | threshold_to_consider | double | Default 0.4. Distance between robot and goal above which prefer forward cost stops being considered         |
+ | threshold_to_consider | double | Default 0.5. Distance between robot and goal above which prefer forward cost stops being considered         |
 
 
 #### Twirling Critic
@@ -183,17 +183,17 @@ controller_server:
         enabled: true
         cost_power: 1
         cost_weight: 5.0
-        threshold_to_consider: 1.0
+        threshold_to_consider: 1.4
       GoalAngleCritic:
         enabled: true
         cost_power: 1
         cost_weight: 3.0
-        threshold_to_consider: 0.4
+        threshold_to_consider: 0.5
       PreferForwardCritic:
         enabled: true
         cost_power: 1
         cost_weight: 5.0
-        threshold_to_consider: 0.4
+        threshold_to_consider: 0.5
       ObstaclesCritic:
         enabled: true
         cost_power: 1
@@ -209,20 +209,20 @@ controller_server:
         cost_weight: 14.0
         max_path_occupancy_ratio: 0.05
         trajectory_point_step: 3
-        threshold_to_consider: 0.40
+        threshold_to_consider: 0.5
         offset_from_furthest: 20
       PathFollowCritic:
         enabled: true
         cost_power: 1
         cost_weight: 5.0
         offset_from_furthest: 5
-        threshold_to_consider: 0.6
+        threshold_to_consider: 1.4
       PathAngleCritic:
         enabled: true
         cost_power: 1
         cost_weight: 2.0
         offset_from_furthest: 4
-        threshold_to_consider: 0.40
+        threshold_to_consider: 0.5
         max_angle_to_furthest: 1.0
         forward_preference: true
       # TwirlingCritic:
@@ -253,7 +253,7 @@ If you don't require path following behavior (e.g. just want to follow a goal po
 
 As this is a predictive planner, there is some relationship between maximum speed, prediction times, and costmap size that users should keep in mind while tuning for their application. If a controller server costmap is set to 3.0m in size, that means that with the robot in the center, there is 1.5m of information on either side of the robot. When your prediction horizon (time_steps * model_dt) at maximum speed (vx_max) is larger than this, then your robot will be artificially limited in its maximum speeds and behavior by the costmap limitation. For example, if you predict forward 3 seconds (60 steps @ 0.05s per step) at 0.5m/s maximum speed, the **minimum** required costmap radius is 1.5m - or 3m total width.
 
-The same applies to the Path Follow and Align offsets from furthest. In the same example if the furthest point we can consider is already at the edge of the costmap, then further offsets are thresholded because they're unusable. So its important while selecting these parameters to make sure that the theoretical offsets can exist on the costmap settings selected with the maximum prediction horizon and velocities desired.
+The same applies to the Path Follow and Align offsets from furthest. In the same example if the furthest point we can consider is already at the edge of the costmap, then further offsets are thresholded because they're unusable. So its important while selecting these parameters to make sure that the theoretical offsets can exist on the costmap settings selected with the maximum prediction horizon and velocities desired. Setting the threshold for consideration in the path follower + goal critics as the same as your prediction horizon can make sure you have clean hand-offs between them, as the path follower will otherwise attempt to slow slightly once it reaches the final goal pose as its marker.
 
 The Path Follow critic cannot drive velocities greater than the projectable distance of that velocity on the available path on the rolling costmap. The Path Align critic `offset_from_furthest` represents the number of path points a trajectory passes through while tracking the path. If this is set either absurdly low (e.g. 5) it can trigger when a robot is simply trying to start path tracking causing some suboptimal behaviors and local minima while starting a task. If it is set absurdly high (e.g. 50) relative to the path resolution and costmap size, then the critic may never trigger or only do so when at full-speed. A balance here is wise. A selection of this value to be ~30% of the maximum velocity distance projected is good (e.g. if a planner produces points every 2.5cm, 60 can fit on the 1.5m local costmap radius. If the max speed is 0.5m/s with a 3s prediction time, then 20 points represents 33% of the maximum speed projected over the prediction horizon onto the path). When in doubt, `prediction_horizon_s * max_speed / path_resolution / 3.0` is a good baseline.
 

nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp

@@ -1,4 +1,5 @@
 // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Open Navigation LLC
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.

nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp

@@ -1,4 +1,5 @@
 // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Open Navigation LLC
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.

nav2_mppi_controller/src/critics/goal_angle_critic.cpp

@@ -24,7 +24,7 @@ void GoalAngleCritic::initialize()
   getParam(power_, "cost_power", 1);
   getParam(weight_, "cost_weight", 3.0);
 
-  getParam(threshold_to_consider_, "threshold_to_consider", 0.40);
+  getParam(threshold_to_consider_, "threshold_to_consider", 0.5);
 
   RCLCPP_INFO(
     logger_,

nav2_mppi_controller/src/critics/goal_critic.cpp

@@ -1,4 +1,5 @@
 // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Open Navigation LLC
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -17,13 +18,15 @@
 namespace mppi::critics
 {
 
+using xt::evaluation_strategy::immediate;
+
 void GoalCritic::initialize()
 {
   auto getParam = parameters_handler_->getParamGetter(name_);
 
   getParam(power_, "cost_power", 1);
   getParam(weight_, "cost_weight", 5.0);
-  getParam(threshold_to_consider_, "threshold_to_consider", 1.0);
+  getParam(threshold_to_consider_, "threshold_to_consider", 1.4);
 
   RCLCPP_INFO(
     logger_, "GoalCritic instantiated with %d power and %f weight.",
@@ -47,14 +50,14 @@ void GoalCritic::score(CriticData & data)
   const auto goal_x = data.path.x(goal_idx);
   const auto goal_y = data.path.y(goal_idx);
 
-  const auto last_x = xt::view(data.trajectories.x, xt::all(), -1);
-  const auto last_y = xt::view(data.trajectories.y, xt::all(), -1);
+  const auto traj_x = xt::view(data.trajectories.x, xt::all(), xt::all());
+  const auto traj_y = xt::view(data.trajectories.y, xt::all(), xt::all());
 
   auto dists = xt::sqrt(
-    xt::pow(last_x - goal_x, 2) +
-    xt::pow(last_y - goal_y, 2));
+    xt::pow(traj_x - goal_x, 2) +
+    xt::pow(traj_y - goal_y, 2));
 
-  data.costs += xt::pow(std::move(dists) * weight_, power_);
+  data.costs += xt::pow(xt::mean(dists, {1}) * weight_, power_);
 }
 
 }  // namespace mppi::critics

nav2_mppi_controller/src/critics/path_align_critic.cpp

@@ -34,7 +34,7 @@ void PathAlignCritic::initialize()
   getParam(trajectory_point_step_, "trajectory_point_step", 4);
   getParam(
     threshold_to_consider_,
-    "threshold_to_consider", 0.40f);
+    "threshold_to_consider", 0.5);
 
   RCLCPP_INFO(
     logger_,

nav2_mppi_controller/src/critics/path_angle_critic.cpp

@@ -37,7 +37,7 @@ void PathAngleCritic::initialize()
   getParam(weight_, "cost_weight", 2.0);
   getParam(
     threshold_to_consider_,
-    "threshold_to_consider", 0.40f);
+    "threshold_to_consider", 0.5);
   getParam(
     max_angle_to_furthest_,
     "max_angle_to_furthest", 1.2);

nav2_mppi_controller/src/critics/path_follow_critic.cpp

@@ -26,7 +26,7 @@ void PathFollowCritic::initialize()
 
   getParam(
     threshold_to_consider_,
-    "threshold_to_consider", 0.40f);
+    "threshold_to_consider", 1.4);
   getParam(offset_from_furthest_, "offset_from_furthest", 6);
   getParam(power_, "cost_power", 1);
   getParam(weight_, "cost_weight", 5.0);

nav2_mppi_controller/src/critics/prefer_forward_critic.cpp

@@ -24,7 +24,7 @@ void PreferForwardCritic::initialize()
   getParam(weight_, "cost_weight", 5.0);
   getParam(
     threshold_to_consider_,
-    "threshold_to_consider", 0.40f);
+    "threshold_to_consider", 0.5);
 
   RCLCPP_INFO(
     logger_, "PreferForwardCritic instantiated with %d power and %f weight.", power_, weight_);

nav2_mppi_controller/test/critics_tests.cpp

@@ -417,9 +417,9 @@ TEST(CriticTests, PathFollowCritic)
   // Scoring testing
 
   // provide state poses and path close within positional tolerances
-  state.pose.pose.position.x = 1.0;
+  state.pose.pose.position.x = 2.0;
   path.reset(6);
-  path.x(5) = 0.85;
+  path.x(5) = 1.7;
   critic.score(data);
   EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);