Refactor alignment importance scaling

Author: rmburg

crates/step_planning/src/cost_fields/target_orientation.rs

@@ -7,38 +7,17 @@ use crate::{
 
 pub struct TargetOrientationField {
     pub target_orientation: Angle<f32>,
-    pub alignment_start_distance: f32,
-    pub ramp_width: f32,
 }
 
 impl TargetOrientationField {
-    pub fn cost(&self, pose: Pose<f32>, progress: f32, path_length: f32) -> f32 {
-        let distance_to_target = path_length - progress;
-
+    pub fn cost(&self, pose: Pose<f32>) -> f32 {
         angle_penalty(pose.orientation, self.target_orientation)
-            * self.importance(distance_to_target)
     }
 
-    pub fn grad(&self, pose: Pose<f32>, progress: f32, path_length: f32) -> PoseGradient<f32> {
-        let distance_to_target = path_length - progress;
-
+    pub fn grad(&self, pose: Pose<f32>) -> PoseGradient<f32> {
         PoseGradient {
             position: Vector2::zeros(),
-            orientation: angle_penalty_derivative(pose.orientation, self.target_orientation)
-                * self.importance(distance_to_target),
-        }
-    }
-}
-
-impl TargetOrientationField {
-    fn importance(&self, distance_to_target: f32) -> f32 {
-        if distance_to_target > self.alignment_start_distance - self.ramp_width {
-            0.0
-        } else if distance_to_target < self.alignment_start_distance + self.ramp_width {
-            1.0
-        } else {
-            (distance_to_target - (self.alignment_start_distance - self.ramp_width))
-                / (2.0 * self.ramp_width)
+            orientation: angle_penalty_derivative(pose.orientation, self.target_orientation),
         }
     }
 }
@@ -49,25 +28,17 @@ mod tests {
 
     use linear_algebra::point;
     use proptest::proptest;
-    use types::planned_path::Path;
 
     use crate::{
         cost_fields::target_orientation::TargetOrientationField,
         geometry::{angle::Angle, pose::Pose},
-        test_utils::test_path,
-        traits::{Length, PathProgress},
     };
 
     proptest!(
         #[test]
         fn verify_gradient(x in -2.0f32..5.0, y in -2.0f32..5.0, orientation in 0.0..TAU) {
             let cost_field = TargetOrientationField {
                 target_orientation: Angle(PI),
-                alignment_start_distance: 1.0,
-                ramp_width: 0.5,
-            };
-            let path =  Path {
-                segments: &test_path(),
             };
 
             let position = point![x, y];
@@ -79,18 +50,8 @@ mod tests {
             };
 
             crate::test_utils::verify_gradient::verify_gradient(
-                &|p: Pose<f32>| {
-                    let progress = path.progress(p.position);
-                    let path_length = path.length();
-
-                    cost_field.cost(p, progress, path_length)
-                },
-                &|p| {
-                    let progress = path.progress(p.position);
-                    let path_length = path.length();
-
-                    cost_field.grad(p, progress, path_length)
-                },
+                &|p| cost_field.cost(p),
+                &|p| cost_field.grad(p),
                 0.05,
                 pose,
             )

crates/step_planning/src/step_plan.rs

@@ -71,15 +71,20 @@ impl StepPlanning<'_> {
 
         let progress = self.path.progress(pose.position);
         let path_length = self.path.length();
+        let distance_to_target = path_length - progress;
+        let target_alignment_importance = self.target_alignment_importance(distance_to_target);
+        let walk_alignment_importance = 1.0 - target_alignment_importance;
 
         let path_progress_cost =
             self.path_progress().cost(progress, path_length) * cost_factors.path_progress;
         let path_distance_cost =
             self.path_distance().cost(pose.position) * cost_factors.path_distance;
-        let walk_orientation_cost =
-            self.walk_orientation().cost(pose.clone()) * cost_factors.walk_orientation;
-        let target_orientation_cost = self.target_orientation().cost(pose, progress, path_length)
-            * cost_factors.target_orientation;
+        let walk_orientation_cost = self.walk_orientation().cost(pose.clone())
+            * cost_factors.walk_orientation
+            * walk_alignment_importance;
+        let target_orientation_cost = self.target_orientation().cost(pose)
+            * cost_factors.target_orientation
+            * target_alignment_importance;
 
         path_progress_cost + path_distance_cost + walk_orientation_cost + target_orientation_cost
     }
@@ -90,16 +95,20 @@ impl StepPlanning<'_> {
         let progress = self.path.progress(pose.position);
         let forward = self.path.forward(pose.position);
         let path_length = self.path.length();
+        let distance_to_target = path_length - progress;
+        let target_alignment_importance = self.target_alignment_importance(distance_to_target);
+        let walk_alignment_importance = 1.0 - target_alignment_importance;
 
         let path_progress_gradient =
             self.path_progress().grad(progress, forward, path_length) * cost_factors.path_progress;
         let path_distance_gradient =
             self.path_distance().grad(pose.position) * cost_factors.path_distance;
-        let walk_orientation_gradient =
-            self.walk_orientation().grad(pose.clone()) * cost_factors.walk_orientation;
-        let target_orientation_gradient =
-            self.target_orientation().grad(pose, progress, path_length)
-                * cost_factors.target_orientation;
+        let walk_orientation_gradient = self.walk_orientation().grad(pose.clone())
+            * cost_factors.walk_orientation
+            * walk_alignment_importance;
+        let target_orientation_gradient = self.target_orientation().grad(pose)
+            * cost_factors.target_orientation
+            * target_alignment_importance;
 
         PoseGradient {
             position: path_progress_gradient + path_distance_gradient,
@@ -108,6 +117,14 @@ impl StepPlanning<'_> {
             + target_orientation_gradient
     }
 
+    // https://www.desmos.com/calculator/mzuvbmrxym
+    fn target_alignment_importance(&self, distance_to_target: f32) -> f32 {
+        (1.0 - f32::tanh(
+            (distance_to_target - self.parameters.alignment_start_distance)
+                * self.parameters.alignment_ramp_steepness,
+        )) / 2.0
+    }
+
     fn path_distance(&self) -> PathDistanceField<'_> {
         PathDistanceField { path: self.path }
     }
@@ -127,8 +144,6 @@ impl StepPlanning<'_> {
     fn target_orientation(&self) -> TargetOrientationField {
         TargetOrientationField {
             target_orientation: Angle(self.target_orientation.angle()),
-            alignment_start_distance: self.parameters.alignment_start_distance,
-            ramp_width: self.parameters.alignment_start_smoothness,
         }
     }
 }

crates/step_planning_solver/benches/mpc_step_planning.rs

@@ -35,7 +35,7 @@ fn plan_steps(path: Path) -> Vec<Step> {
             },
             path_progress_smoothness: 0.05,
             alignment_start_distance: 0.1,
-            alignment_start_smoothness: 0.05,
+            alignment_ramp_steepness: 50.0,
         };
 
     let (step_plan, _, _) = step_planning_solver::plan_steps(

crates/types/src/parameters.rs

@@ -475,5 +475,5 @@ pub struct StepPlanningOptimizationParameters {
     pub cost_factors: StepPlanningCostFactors,
     pub path_progress_smoothness: f32,
     pub alignment_start_distance: f32,
-    pub alignment_start_smoothness: f32,
+    pub alignment_ramp_steepness: f32,
 }

etc/parameters/default.json

@@ -406,7 +406,7 @@
       },
       "path_progress_smoothness": 0.05,
       "alignment_start_distance": 0.1,
-      "alignment_start_smoothness": 0.05
+      "alignment_ramp_steepness": 50
     }
   },
   "zero_moment_point": {