Separate collision avoidance from desired object clearance

trajectory_optimization/README.md

@@ -59,9 +59,9 @@ flowchart LR
 | `run_as_callback` | `bool` | `false` | Run OCP once for each received reference trajectory (true) or on a timer (false) |
 | `cost_weights` | `float[]` | `std::vector<double>(12, 1.0)` | Cost function weights |
 | `dynamic_weight` | `float` | `1.0` | Dynamic weight alpha |
-| `thw` | `float` | `2.0` | Time headway to front vehicle |
-| `d_min_object_long` | `float` | `5.0` | Minimum distance to keep to objects in longitudinal direction [m] |
-| `d_min_object_lat` | `float` | `0.5` | Minimum distance to keep to objects in lateral direction [m] |
+| `thw` | `float` | `2.0` | Desired time headway to front vehicle [s]; soft when object slacks are enabled |
+| `d_min_object_long` | `float` | `5.0` | Desired longitudinal object clearance [m]; soft when object slacks are enabled |
+| `d_min_object_lat` | `float` | `0.5` | Desired lateral object clearance [m]; soft when object slacks are enabled |
 | `d_min_boundary_lat` | `float` | `0.0` | Minimum distance to keep to boundary in lateral direction [m] |
 | `standstill_threshold` | `float` | `0.45` | Threshold for standstill detection [m/s]. If the velocities of all states are below this threshold, publish standstill trajectory |
 | `high_level_stabilization` | `bool` | `false` | Use high-level stabilization strategy for init state (= init with current EgoData) |

trajectory_optimization/config/example_params_ackermann.yml

@@ -47,7 +47,7 @@
     dynamic_weight: 1.0         # base for exponential weight
 
     # other cost params
-    thw: 2.0                    # time headway [s]
-    d_min_object_long: 1.0      # minimum distance to keep to objects in longitudinal direction [m]
-    d_min_object_lat: 0.1       # minimum distance to keep to objects in lateral direction [m]
+    thw: 2.0                    # desired time headway [s]; soft with object slacks
+    d_min_object_long: 1.0      # desired longitudinal object clearance [m]; soft with object slacks
+    d_min_object_lat: 0.1       # desired lateral object clearance [m]; soft with object slacks
     d_min_boundary_lat: 0.0      # minimum distance to keep to boundaries in lateral direction [m]

trajectory_optimization/config/example_params_rws.yml

@@ -51,7 +51,7 @@
     dynamic_weight: 1.0         # base for exponential weight
 
     # other cost params
-    thw: 2.0                    # time headway [s]
-    d_min_object_long: 1.0      # minimum distance to keep to objects in longitudinal direction [m]
-    d_min_object_lat: 0.1       # minimum distance to keep to objects in lateral direction [m]
+    thw: 2.0                    # desired time headway [s]; soft with object slacks
+    d_min_object_long: 1.0      # desired longitudinal object clearance [m]; soft with object slacks
+    d_min_object_lat: 0.1       # desired lateral object clearance [m]; soft with object slacks
     d_min_boundary_lat: 0.0      # minimum distance to keep to boundaries in lateral direction [m]

trajectory_optimization/include/trajectory_optimization/performance_logger.hpp

@@ -61,6 +61,9 @@ struct PerformanceMetrics {
   double max_eq_violation = 0.0;
   int max_eq_stage = -1;
   int max_eq_state = -1;
+  double max_object_clearance_slack = 0.0;
+  int max_object_clearance_slack_stage = -1;
+  int max_object_clearance_slack_index = -1;
 };
 
 class PerformanceLogger {
@@ -139,6 +142,26 @@ class PerformanceLogger {
                                            const ocp_nlp_dims* dims,
                                            int object_boxes);
 
+  /**
+   * @brief Checks all slacks for finiteness and optionally collects desired-clearance statistics.
+   *
+   * @param[in,out] metrics Metrics populated with the largest lower object-clearance slack.
+   * @param[in] config acados NLP configuration.
+   * @param[in] dims acados NLP dimensions.
+   * @param[in] input acados NLP input containing the slack-to-constraint mapping.
+   * @param[in] output acados NLP output containing the slack values.
+   * @param[in] object_boxes Number of object boxes in each object constraint block.
+   * @param[in] collect_details Whether to collect object-clearance statistics.
+   * @return True if all lower and upper slack values are finite.
+   */
+  static bool collectSlackStatistics(PerformanceMetrics& metrics,
+                                     ocp_nlp_config* config,
+                                     ocp_nlp_dims* dims,
+                                     ocp_nlp_in* input,
+                                     ocp_nlp_out* output,
+                                     int object_boxes,
+                                     bool collect_details);
+
   /**
    * @brief Returns the path of the CSV performance log.
    *

trajectory_optimization/src/performance_logger.cpp

@@ -61,7 +61,8 @@ PerformanceLogger::PerformanceLogger(const std::string& node_name) {
          "acados_qp_solver_ms,acados_qp_xcond_ms,acados_reg_ms,acados_glob_ms,acados_preparation_ms,"
          "acados_feedback_ms,cost,kkt,nlp_res,res_stat,res_eq,res_ineq,res_comp,"
          "max_ineq_violation,max_ineq_stage,max_ineq_type,max_ineq_index,max_ineq_side,"
-         "max_eq_violation,max_eq_stage,max_eq_state\n";
+         "max_eq_violation,max_eq_stage,max_eq_state,"
+         "max_object_clearance_slack,max_object_clearance_slack_stage,max_object_clearance_slack_index\n";
   stream_.flush();
 }
 
@@ -169,11 +170,14 @@ void PerformanceLogger::collectConstraintDiagnostics(PerformanceMetrics& metrics
       metrics.max_ineq_type = h_index == 0 ? "boundary_left" : "boundary_right";
       metrics.max_ineq_index = 0;
     } else if (h_index < object_boxes + 2) {
-      metrics.max_ineq_type = "object";
+      metrics.max_ineq_type = "object_clearance";
       metrics.max_ineq_index = h_index - 2;
+    } else if (h_index < 2 * object_boxes + 2) {
+      metrics.max_ineq_type = "object_collision";
+      metrics.max_ineq_index = h_index - object_boxes - 2;
     } else {
       metrics.max_ineq_type = "vehicle";
-      metrics.max_ineq_index = h_index - object_boxes - 2;
+      metrics.max_ineq_index = h_index - 2 * object_boxes - 2;
     }
   } else {
     metrics.max_ineq_type = "slack";
@@ -182,8 +186,45 @@ void PerformanceLogger::collectConstraintDiagnostics(PerformanceMetrics& metrics
   // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic)
 }
 
+bool PerformanceLogger::collectSlackStatistics(PerformanceMetrics& metrics,
+                                               ocp_nlp_config* config,
+                                               ocp_nlp_dims* dims,
+                                               ocp_nlp_in* input,
+                                               ocp_nlp_out* output,
+                                               int object_boxes,
+                                               bool collect_details) {
+  metrics.max_object_clearance_slack = 0.0;
+  metrics.max_object_clearance_slack_stage = -1;
+  metrics.max_object_clearance_slack_index = -1;
+  bool finite_slacks = true;
+  // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic)
+  for (int stage = 0; stage <= dims->N; ++stage) {
+    const int ns = dims->ns[stage];
+    if (ns == 0) continue;
+    std::vector<double> lower(ns), upper(ns);
+    ocp_nlp_out_get(config, dims, output, stage, "sl", lower.data());
+    ocp_nlp_out_get(config, dims, output, stage, "su", upper.data());
+    finite_slacks = finite_slacks && std::all_of(lower.begin(), lower.end(), [](double value) { return std::isfinite(value); }) &&
+                    std::all_of(upper.begin(), upper.end(), [](double value) { return std::isfinite(value); });
+    if (!collect_details) continue;
+    std::vector<int> slack_indices(ns);
+    ocp_nlp_constraints_model_get(config, dims, input, stage, "idxs", slack_indices.data());
+    const int clearance_start = dims->nb[stage] + dims->ng[stage] + 2;
+    for (int slack = 0; slack < ns; ++slack) {
+      const int object_index = slack_indices[slack] - clearance_start;
+      if (object_index >= 0 && object_index < object_boxes && lower[slack] > metrics.max_object_clearance_slack) {
+        metrics.max_object_clearance_slack = lower[slack];
+        metrics.max_object_clearance_slack_stage = stage;
+        metrics.max_object_clearance_slack_index = object_index;
+      }
+    }
+  }
+  // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic)
+  return finite_slacks;
+}
+
 void PerformanceLogger::write(const PerformanceMetrics& metrics) {
-  stream_ << std::setprecision(17) << 6 << ",runtime,," << metrics.cycle << ',' << nowNanoseconds() << ',' << metrics.ego_stamp_ns
+  stream_ << std::setprecision(17) << 7 << ",runtime,," << metrics.cycle << ',' << nowNanoseconds() << ',' << metrics.ego_stamp_ns
           << ',' << metrics.reference_stamp_ns << ',' << metrics.route_stamp_ns << ',' << metrics.status << ','
           << (metrics.published ? 1 : 0) << ',' << metrics.reference_points << ',' << metrics.objects << ','
           << (metrics.relaxed_attempted ? 1 : 0) << ',' << metrics.failure_phase << ',' << metrics.relaxed_status << ','
@@ -197,7 +238,8 @@ void PerformanceLogger::write(const PerformanceMetrics& metrics) {
           << metrics.res_eq << ',' << metrics.res_ineq << ',' << metrics.res_comp << ',' << metrics.max_ineq_violation << ','
           << metrics.max_ineq_stage << ',' << metrics.max_ineq_type << ',' << metrics.max_ineq_index << ','
           << metrics.max_ineq_side << ',' << metrics.max_eq_violation << ',' << metrics.max_eq_stage << ','
-          << metrics.max_eq_state << '\n';
+          << metrics.max_eq_state << ',' << metrics.max_object_clearance_slack << ',' << metrics.max_object_clearance_slack_stage
+          << ',' << metrics.max_object_clearance_slack_index << '\n';
 
   if (++records_since_flush_ >= FLUSH_INTERVAL) {
     stream_.flush();

trajectory_optimization/src/trajectory_optimization_node.cpp

@@ -63,11 +63,11 @@ TrajectoryOptimizationNode::TrajectoryOptimizationNode(const std::string node_na
                                 "Run OCP once for each received reference trajectory (true) or on a timer (false)");
   this->declareAndLoadParameter("cost_weights", cost_weights_, "Cost function weights");
   this->declareAndLoadParameter("dynamic_weight", dynamic_weight_, "Dynamic weight alpha");
-  this->declareAndLoadParameter("thw", thw_, "Time headway to front vehicle");
+  this->declareAndLoadParameter("thw", thw_, "Desired time headway to front vehicle [s]; soft when object slacks are enabled");
   this->declareAndLoadParameter("d_min_object_long", d_min_object_long_,
-                                "Minimum distance to keep to objects in longitudinal direction [m]");
+                                "Desired longitudinal object clearance [m]; soft when object slacks are enabled");
   this->declareAndLoadParameter("d_min_object_lat", d_min_object_lat_,
-                                "Minimum distance to keep to objects in lateral direction [m]");
+                                "Desired lateral object clearance [m]; soft when object slacks are enabled");
   this->declareAndLoadParameter("d_min_boundary_lat", d_min_boundary_lat_,
                                 "Minimum distance to keep to boundary in lateral direction [m]");
   this->declareAndLoadParameter("standstill_threshold", standstill_threshold_,

trajectory_optimization/src/utils.cpp

@@ -427,7 +427,11 @@ bool TrajectoryOptimizationNode::solveAndValidate(PerformanceMetrics& metrics, b
     }
   }
 
-  finite_solution = usable_status && std::isfinite(metrics.cost_value) && std::isfinite(metrics.res_eq) &&
+  const bool finite_slacks =
+      usable_status && PerformanceLogger::collectSlackStatistics(metrics, nlp_config_, nlp_dims_, nlp_in_, nlp_out_,
+                                                                 static_cast<int>(p_objects_shape_[0]),
+                                                                 performance_logger_ != nullptr || verbose_);
+  finite_solution = usable_status && finite_slacks && std::isfinite(metrics.cost_value) && std::isfinite(metrics.res_eq) &&
                     std::isfinite(metrics.res_ineq) &&
                     std::all_of(xtraj_.begin(), xtraj_.end(), [](double value) { return std::isfinite(value); }) &&
                     std::all_of(utraj_.begin(), utraj_.end(), [](double value) { return std::isfinite(value); });

trajectory_optimization_ocp/README.md

@@ -36,6 +36,24 @@ Model/config parameterizations are defined in:
 - [`trajectory_optimization_ocp/config/karl_params.yml`](trajectory_optimization_ocp/config/karl_params.yml)
 - [`trajectory_optimization_ocp/config/shuttle_params.yml`](trajectory_optimization_ocp/config/shuttle_params.yml)
 
+### Object constraints
+
+Both vehicle OCPs separate desired clearance from physical collision avoidance:
+
+- Desired clearance: the ego box expanded by THW/`d_min_object_long` to the front,
+  `d_min_object_lat` to the sides and a small rear margin. `enable_object_slack` softens
+  this block using `object_slack_weights`; without slacks it stays hard.
+- Physical separation: the unexpanded ego and object boxes must be separated by the build
+  parameter `d_min_object_collision` (default 0.10 m). This block follows the clearance
+  block and never receives slacks.
+
+Both blocks use the conservative smooth SAT margin and apply at intermediate and terminal
+nodes, not retroactively at the fixed initial state. Node constraints alone do not guarantee
+collision avoidance between nodes or under incorrect predictions. `thw`, `d_min_object_long`
+and `d_min_object_lat` are runtime parameters; changing `d_min_object_collision` or the slack
+weights requires rebuilding the generated solver. Generation errors fail the build instead of
+reusing stale solver artifacts.
+
 ### Forward-only dynamics
 
 Both bicycle models use `v_dot = max(a, -v / tau_stop)` with `tau_stop = 0.05 s`.

trajectory_optimization_ocp/codegeneration.cmake

@@ -21,8 +21,10 @@ set(GENERATOR generate_ocp.py)
 # Needs to be generated in different processes
 execute_process(
   COMMAND bash "-c" "python ${CMAKE_CURRENT_BINARY_DIR}/${GENERATOR} --config ${CMAKE_CURRENT_BINARY_DIR}/karl_params.yml"
+  COMMAND_ERROR_IS_FATAL ANY
 )
 
 execute_process(
   COMMAND bash "-c" "python ${CMAKE_CURRENT_BINARY_DIR}/${GENERATOR} --config ${CMAKE_CURRENT_BINARY_DIR}/shuttle_params.yml"
+  COMMAND_ERROR_IS_FATAL ANY
 )

trajectory_optimization_ocp/trajectory_optimization_ocp/config/karl_params.yml

@@ -21,6 +21,7 @@ acceleration_n_max: 3.0           # max. absolute normal acceleration [m/s²]
 alpha_max: 0.7330382858           # max. absolute steering rate [rad/s] (42°/s)
 jerk_min: -5.0                    # max. negative tangential jerk [m/s²]
 jerk_max: 2.0                     # max. positive tangential jerk [m/s²]
+d_min_object_collision: 0.10      # hard minimum separation of the physical OBBs [m]; requires solver rebuild
 
 ### cost scaling factors
 c_lat: 0.5            # distance to scale the lateral deviation costs [m]
@@ -40,7 +41,7 @@ boundary_slack_weights:                     # scalar or list (length 2)
   linear_upper: 10.0
   quadratic_lower: 100.0
   quadratic_upper: 100.0
-enable_object_slack: False                  # activate slack weights for object constraints
+enable_object_slack: True                   # soften desired THW/lateral clearance; physical separation stays hard
 object_slack_weights:                       # scalar or list (length n_objects)
   linear_lower: 50.0
   linear_upper: 50.0

trajectory_optimization_ocp/trajectory_optimization_ocp/config/shuttle_params.yml

@@ -24,6 +24,7 @@ acceleration_t_max: 3.5           # max. positive tangential acceleration [m/s²
 acceleration_n_max: 8.0           # max. absolute normal acceleration [m/s²]
 jerk_min: -4.0                    # max. negative tangential jerk [m/s²]
 jerk_max: 4.0                     # max. positive tangential jerk [m/s³]
+d_min_object_collision: 0.10      # hard minimum separation of the physical OBBs [m]; requires solver rebuild
 # constraint on front and rear steering angle
 # none (default): no constraint, ICR (instantaneous center of rotation) is free to move
 # anti-phase: ICR is restricted to the y-axis in the vehicles center of gravity frame -> achieves minimum turning radius
@@ -48,7 +49,7 @@ boundary_slack_weights:                     # scalar or list (length 2)
   linear_upper: 10.0
   quadratic_lower: 100.0
   quadratic_upper: 100.0
-enable_object_slack: False                  # activate slack weights for object constraints
+enable_object_slack: True                   # soften desired THW/lateral clearance; physical separation stays hard
 object_slack_weights:                       # scalar or list (length n_objects)
   linear_lower: 50.0
   linear_upper: 50.0

trajectory_optimization_ocp/trajectory_optimization_ocp/constraints.py

@@ -161,12 +161,19 @@ def set_constraints(ocp: AcadosOcp, config):
         np.prod(config["p_dynamic_weight_shape"]) + np.prod(config["p_constraint_activation_shape"]) + np.prod(object_shape)
     )
 
+    # Hard minimum separation of the physical boxes; never relaxed by slacks.
+    d_min_object_collision = float(config["d_min_object_collision"])
+    if not np.isfinite(d_min_object_collision) or d_min_object_collision < 0.0:
+        raise ValueError("d_min_object_collision must be finite and nonnegative")
+
     v_t = ocp.model.x[STATE_INDEX_V_T]
+    # Desired clearance box, soft when object slacks are enabled.
     # Both vehicle models are forward-only. THW and d_min_object_long protect
     # the front; the rear only needs a small physical clearance.
     front_margin = ca.fmax(ca.fmax(d_min_object_long, 0.0), p_thw * ca.fmax(v_t, 0.0))
     lateral_margin = ca.fmax(d_min_object_lat, 0.0)
-    safety_ego_obb = expand_ego_obb_forward(ego_obb, front_margin, OBB_REAR_MARGIN_M, lateral_margin)
+    clearance_obb = expand_ego_obb_forward(ego_obb, front_margin, OBB_REAR_MARGIN_M, lateral_margin)
+    collision_constraints = []
     for i in range(object_shape[0]):
         base = i * object_shape[1]
         object_yaw = p_objects[base + P_OBJECTS_INDEX_YAW]
@@ -178,13 +185,21 @@ def set_constraints(ocp: AcadosOcp, config):
             "half_length": p_objects[base + P_OBJECTS_INDEX_HALF_LENGTH],
             "half_width": p_objects[base + P_OBJECTS_INDEX_HALF_WIDTH],
         }
-        sat_margin = conservative_smooth_sat_margin(safety_ego_obb, object_box, GEOMETRY_SMOOTHING_EPSILON_M, SAT_SMOOTHING_TAU_M)
+        sat_margin = conservative_smooth_sat_margin(clearance_obb, object_box, GEOMETRY_SMOOTHING_EPSILON_M, SAT_SMOOTHING_TAU_M)
         sat_margin = saturate_positive_margin(sat_margin, SAT_POSITIVE_MARGIN_SCALE_M)
         active = p_object_activation * p_objects[base + P_OBJECTS_INDEX_ACTIVE]
         constraint = activate_constraint(sat_margin, active)
         ocp.model.con_h_expr = ca.vertcat(ocp.model.con_h_expr, constraint)
         cons.lh = np.concatenate((cons.lh, [0.0]))
         cons.uh = np.concatenate((cons.uh, [ACADOS_INFTY]))
+        collision_margin = conservative_smooth_sat_margin(ego_obb, object_box, GEOMETRY_SMOOTHING_EPSILON_M, SAT_SMOOTHING_TAU_M)
+        collision_margin = saturate_positive_margin(collision_margin - d_min_object_collision, SAT_POSITIVE_MARGIN_SCALE_M)
+        collision_constraints.append(activate_constraint(collision_margin, active))
+
+    # The hard collision constraints follow as a separate block that never receives slacks.
+    ocp.model.con_h_expr = ca.vertcat(ocp.model.con_h_expr, *collision_constraints)
+    cons.lh = np.concatenate((cons.lh, np.zeros(len(collision_constraints))))
+    cons.uh = np.concatenate((cons.uh, np.full(len(collision_constraints), ACADOS_INFTY)))
 
     # --- a_abs_squared, psi_dot, steering_mode_constraint ---
     # Ackermann: psi_dot = v / l * tan(delta_f)

utils/README.md

@@ -29,11 +29,19 @@ Each row describes one completed planning cycle:
   residuals.
 - **Rejected solutions:** largest constraint or dynamics violation including
   shooting stage, constraint type, index, and bound side.
+- **Desired clearance:** largest lower object-clearance slack in meters, with stage
+  and object-slot index, including accepted solutions (CSV schema 7). Boundary slacks
+  are excluded. Object constraint diagnostics distinguish `object_collision` from
+  `object_clearance`; positive clearance slacks do not relax physical separation.
 
 Detailed constraint diagnostics are collected only for finite solver outputs
 that are rejected. CSV writing happens after cycle timing and is therefore not
 included in `cycle_ms`.
 
+All lower/upper slacks are checked for finiteness before accepting solver output.
+Clearance statistics are collected for usable solver statuses when logging or verbose
+output is enabled; stage/index are -1 when no positive object-clearance slack is found.
+
 Analyze a run:
 
 ```bash