@@ -21,6 +21,7 @@ class ObjectiveConfig:
2121 terrain_violation_weight: Weight for terrain violations.
2222 convergence_tol: Normalized distance threshold for convergence.
2323 boundary_tol: Normalized distance for boundary violation.
24+ efficiency_threshold: Minimum efficiency threshold for path efficiency.
2425 terrain_violation_tol: Normalized distance threshold to check for terrain violation.
2526 terrain_violation_accuracy: Number of points to check for terrain violation.
2627 lucky_jump_threshold: Maximum allowed step size (fraction of diagonal).
@@ -32,14 +33,15 @@ class ObjectiveConfig:
3233 final_val_weight : float = 1.8
3334 final_dist_weight : float = 2.6
3435 convergence_weight : float = 0.1
35- efficiency_weight : float = 0.1
36+ efficiency_weight : float = 0.8
3637 lucky_jump_weight : float = 10.0
3738 start_prox_weight : float = 200.0
3839 boundary_weight : float = 16.0
3940 terrain_violation_weight : float = 20.0
4041
4142 convergence_tol : float = 0.01
4243 boundary_tol : float = 0.08
44+ efficiency_threshold : float = 1.4
4345 terrain_violation_tol : float = 0.01
4446 terrain_violation_accuracy : int = 7
4547 lucky_jump_threshold : float = 0.04
@@ -71,26 +73,34 @@ def _calc_boundary_violation(
7173
7274
7375def _calc_path_inefficiency (
74- steps : torch .Tensor , step_lengths : torch .Tensor
76+ steps : torch .Tensor ,
77+ step_lengths : torch .Tensor ,
78+ threshold : float ,
7579) -> torch .Tensor :
76- """Compute path inefficiency as (total length / displacement) - 1."""
77- path_len = torch .sum (step_lengths )
78- displacement = torch .norm (steps [:, - 1 ] - steps [:, 0 ])
79-
80- if displacement < 1e-6 :
81- return torch .tensor (0.0 )
80+ """Calculates path inefficiency by comparing the spatial footprint against significant movement effort."""
81+ # Footprint: The diagonal of the bounding box touched by the optimizer
82+ bbox_max = torch .max (steps , dim = 1 ).values
83+ bbox_min = torch .min (steps , dim = 1 ).values
84+ span = torch .norm (bbox_max - bbox_min )
8285
83- return torch .relu ((path_len / displacement ) - 1.0 )
86+ # Jitter Filter: Ignore steps that are mathematically insignificant
87+ # relative to the optimizer's largest movement (active phase).
88+ max_s = torch .max (step_lengths )
89+ active_mask = step_lengths > (max_s * 0.01 ) # Ignore steps < 1% of peak velocity
90+ significant_effort = torch .sum (step_lengths [active_mask ])
8491
92+ # Raw Efficiency: Ratio of ground covered to significant energy spent.
93+ # threshold acts as a 'Curvature Buffer' (e.g., 1.5 allows a path 50% longer than a straight line).
94+ raw_efficiency = (span * threshold ) / (significant_effort + 1e-12 )
8595
86- def _calc_lucky_jump (step_lengths : torch .Tensor , threshold : float ) -> torch .Tensor :
87- """Compute penalty for steps exceeding the threshold."""
88- max_step = torch .max (step_lengths )
96+ return 1.0 - torch .clamp (raw_efficiency , max = 1.0 )
8997
90- if max_step > threshold :
91- return (max_step - threshold ) ** 2
9298
93- return torch .tensor (0.0 )
99+ def _calc_lucky_jump (step_lengths : torch .Tensor , threshold : float ) -> torch .Tensor :
100+ """Compute cumulative penalty for steps exceeding the threshold."""
101+ excess = step_lengths - threshold
102+ penalty = torch .clamp (excess , min = 0.0 )
103+ return torch .sum (penalty ** 2 )
94104
95105
96106def _calc_start_proximity (
@@ -275,8 +285,12 @@ def objective(
275285
276286 # Path inefficiency
277287 if config .efficiency_weight > 0 :
278- inefficiency = _calc_path_inefficiency (steps , step_lengths ).item ()
279- eff_penalty = min (inefficiency , 10.0 ) * config .efficiency_weight
288+ inefficiency = _calc_path_inefficiency (
289+ steps ,
290+ step_lengths ,
291+ config .efficiency_threshold ,
292+ ).item ()
293+ eff_penalty = inefficiency * config .efficiency_weight
280294 metrics ["eff_penalty" ] = eff_penalty
281295 error_sum += eff_penalty
282296
0 commit comments