From 9a43695462230eb4f943761e97d9fc43cdf61f20 Mon Sep 17 00:00:00 2001 From: JasonZhou404 Date: Thu, 21 Nov 2019 11:25:10 -0800 Subject: [PATCH] Planning: use soft bound in nlp speed opt --- modules/planning/conf/planning_config.pb.txt | 1 + .../piecewise_jerk_nonlinear_speed_config.proto | 1 + .../piecewise_jerk_speed_nonlinear_optimizer.cc | 15 ++++++++++++++- .../piecewise_jerk_speed_nonlinear_optimizer.h | 1 + 4 files changed, 17 insertions(+), 1 deletion(-) diff --git a/modules/planning/conf/planning_config.pb.txt b/modules/planning/conf/planning_config.pb.txt index dc9ad532d8..93b591e3c9 100644 --- a/modules/planning/conf/planning_config.pb.txt +++ b/modules/planning/conf/planning_config.pb.txt @@ -38,6 +38,7 @@ default_task_config: { lat_acc_weight: 1000.0 ref_v_weight: 0.0 ref_s_weight: 1000.0 + soft_s_bound_weight: 1e9 use_warm_start: true } } diff --git a/modules/planning/proto/piecewise_jerk_nonlinear_speed_config.proto b/modules/planning/proto/piecewise_jerk_nonlinear_speed_config.proto index b7b6676260..c70497f12f 100644 --- a/modules/planning/proto/piecewise_jerk_nonlinear_speed_config.proto +++ b/modules/planning/proto/piecewise_jerk_nonlinear_speed_config.proto @@ -11,6 +11,7 @@ message PiecewiseJerkNonlinearSpeedConfig { optional double end_s_weight = 6 [default = 10.0]; optional double end_v_weight = 7 [default = 10.0]; optional double end_a_weight = 8 [default = 10.0]; + optional double soft_s_bound_weight = 9 [default = 10.0]; optional bool use_warm_start = 100 [default = true]; } diff --git a/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_optimizer.cc b/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_optimizer.cc index d34bc82e0d..6ddd16cb6f 100644 --- a/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_optimizer.cc +++ b/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_optimizer.cc @@ -214,10 +214,16 @@ Status PiecewiseJerkSpeedNonlinearOptimizer::SetUpStatesAndBounds( // Set s boundary s_bounds_.clear(); + s_soft_bounds_.clear(); + // TODO(Jinyun): soft bound only takes effect in follow fence, will use in + // other speed decision as well and use more sophisticated bound for follow + // fence for (int i = 0; i < num_of_knots_; ++i) { double curr_t = i * delta_t_; double s_lower_bound = 0.0; double s_upper_bound = total_length_; + double s_soft_lower_bound = 0.0; + double s_soft_upper_bound = 0.0; for (const STBoundary* boundary : st_graph_data.st_boundaries()) { double s_lower = 0.0; double s_upper = 0.0; @@ -228,13 +234,16 @@ Status PiecewiseJerkSpeedNonlinearOptimizer::SetUpStatesAndBounds( case STBoundary::BoundaryType::STOP: case STBoundary::BoundaryType::YIELD: s_upper_bound = std::fmin(s_upper_bound, s_upper); + s_soft_lower_bound = s_upper_bound; break; case STBoundary::BoundaryType::FOLLOW: // TODO(Hongyi): unify follow buffer on decision side - s_upper_bound = std::fmin(s_upper_bound, s_upper - 8.0); + s_upper_bound = std::fmin(s_upper_bound, s_upper); + s_soft_upper_bound = s_upper_bound - 8.0; break; case STBoundary::BoundaryType::OVERTAKE: s_lower_bound = std::fmax(s_lower_bound, s_lower); + s_soft_lower_bound = s_lower_bound; break; default: break; @@ -245,6 +254,7 @@ Status PiecewiseJerkSpeedNonlinearOptimizer::SetUpStatesAndBounds( AERROR << msg; return Status(ErrorCode::PLANNING_ERROR, msg); } + s_soft_bounds_.emplace_back(s_soft_lower_bound, s_soft_upper_bound); s_bounds_.emplace_back(s_lower_bound, s_upper_bound); } speed_limit_ = st_graph_data.speed_limit(); @@ -428,6 +438,8 @@ Status PiecewiseJerkSpeedNonlinearOptimizer::OptimizeByNLP( ptr_interface->set_safety_bounds(s_bounds_); + ptr_interface->set_soft_safety_bounds(s_soft_bounds_); + // Set weights and reference values const auto& config = config_.piecewise_jerk_nonlinear_speed_config(); @@ -471,6 +483,7 @@ Status PiecewiseJerkSpeedNonlinearOptimizer::OptimizeByNLP( ptr_interface->set_w_overall_j(config.jerk_weight()); ptr_interface->set_w_overall_centripetal_acc(config.lat_acc_weight()); ptr_interface->set_w_reference_speed(config.ref_v_weight()); + ptr_interface->set_w_soft_s_bound(config.soft_s_bound_weight()); Ipopt::SmartPtr problem = ptr_interface; Ipopt::SmartPtr app = IpoptApplicationFactory(); diff --git a/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_optimizer.h b/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_optimizer.h index 509456b08e..82cf8b9b05 100644 --- a/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_optimizer.h +++ b/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_optimizer.h @@ -77,6 +77,7 @@ class PiecewiseJerkSpeedNonlinearOptimizer : public SpeedOptimizer { // st safety bounds std::vector> s_bounds_; + std::vector> s_soft_bounds_; // speed limits SpeedLimit speed_limit_; -- GitLab