feat(obstacle_avoidance_planner): faster optimization by sparser re-formulation #4600

takayuki5168 · 2023-08-11T18:31:38Z

Description

According to https://github.com/orgs/autowarefoundation/discussions/3681, this PR makes the optimization of MPT (Model Predictive Trajectory) sparser resulting in faster calculation.

TODO

check the result with debug build

Qualitative evaluation

I measured the calculation time of obstacle_avoidance_planner with the following route on the sample map.

without this PR

with this PR
The time rarely increases suddenly, but overall much better than without this PR.

Tests performed

planning simulator

Effects on system behavior

Nothing

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

I've confirmed the contribution guidelines.
The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

The PR follows the pull request guidelines.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

takayuki5168 · 2023-08-12T05:06:11Z

planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp

@@ -1522,7 +1506,9 @@ std::optional<Eigen::VectorXd> MPTOptimizer::calcOptimizedSteerAngles(
  const autoware::common::osqp::CSC_Matrix P_csc =
    autoware::common::osqp::calCSCMatrixTrapezoidal(H);
  const autoware::common::osqp::CSC_Matrix A_csc = autoware::common::osqp::calCSCMatrix(A);
-  if (mpt_param_.enable_warm_start && prev_mat_n_ == H.rows() && prev_mat_m_ == A.rows()) {
+  if (
+    prev_solution_status_ == 1 && mpt_param_.enable_warm_start && prev_mat_n_ == H.rows() &&


Without prev_solution_status_ == 1, the warm start is used even if the previous optimization failed, resulting in successive optimization failure.

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

codecov · 2023-08-12T06:48:43Z

Codecov Report

Patch coverage: 50.00% and project coverage change: -0.01% ⚠️

Comparison is base (2b7afb1) 14.84% compared to head (4a0979a) 14.83%.
Report is 1 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #4600      +/-   ##
==========================================
- Coverage   14.84%   14.83%   -0.01%     
==========================================
  Files        1529     1529              
  Lines      105769   105761       -8     
  Branches    32221    32215       -6     
==========================================
- Hits        15698    15694       -4     
+ Misses      72913    72912       -1     
+ Partials    17158    17155       -3

Flag	Coverage Δ		*Carryforward flag
differential	`38.68% <50.00%> (?)`
total	`14.81% <ø> (-0.03%)`	⬇️	Carriedforward from 2b7afb1

*This pull request uses carry forward flags. Click here to find out more.

Files Changed	Coverage Δ
...ule/dynamic_avoidance/dynamic_avoidance_module.hpp	`0.00% <ø> (ø)`
...ule/dynamic_avoidance/dynamic_avoidance_module.cpp	`0.00% <ø> (ø)`
...ner/src/scene_module/dynamic_avoidance/manager.cpp	`0.00% <ø> (ø)`
...clude/obstacle_avoidance_planner/mpt_optimizer.hpp	`85.71% <ø> (ø)`
...cle_avoidance_planner/state_equation_generator.hpp	`100.00% <ø> (ø)`
...g/obstacle_avoidance_planner/src/mpt_optimizer.cpp	`40.11% <44.77%> (+0.11%)`	⬆️
...avoidance_planner/src/state_equation_generator.cpp	`65.21% <81.81%> (-3.75%)`	⬇️

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

…ormulation (autowarefoundation#4600) * feat(obstacle_avoidance_planner): re-formulate for faster QP Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * minor change Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

…ormulation (autowarefoundation#4600) (#1051) * feat(obstacle_avoidance_planner): re-formulate for faster QP * minor change * update * fix --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

…ormulation (autowarefoundation#4600) (autowarefoundation#1051) * feat(obstacle_avoidance_planner): re-formulate for faster QP * minor change * update * fix --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

github-actions bot added the component:planning Route planning, decision-making, and navigation. (auto-assigned) label Aug 11, 2023

takayuki5168 added 2 commits August 12, 2023 13:06

feat(obstacle_avoidance_planner): re-formulate for faster QP

b7aed3f

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

minor change

74dcb2a

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

takayuki5168 changed the title ~~Feat/fast optimization path planner~~ feat(obstacle_avoidance_planner): faster optimization by sparser re-formulation Aug 12, 2023

update

182f490

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

takayuki5168 force-pushed the feat/fast-optimization-path-planner branch from e0405e9 to 182f490 Compare August 12, 2023 04:54

takayuki5168 marked this pull request as ready for review August 12, 2023 05:04

takayuki5168 requested review from kosuke55 and TakaHoribe as code owners August 12, 2023 05:04

takayuki5168 added the run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Aug 12, 2023

takayuki5168 commented Aug 12, 2023

View reviewed changes

takayuki5168 assigned kosuke55 Aug 12, 2023

kosuke55 approved these changes Aug 12, 2023

View reviewed changes

fix

4a0979a

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

takayuki5168 merged commit 9ac3a7c into autowarefoundation:main Aug 12, 2023

takayuki5168 deleted the feat/fast-optimization-path-planner branch August 12, 2023 14:27

tier4-autoware-public-bot bot mentioned this pull request Aug 14, 2023

chore: sync upstream tier4/autoware.universe#724

Merged

rej55 mentioned this pull request Aug 18, 2023

feat(obstacle_avoidance_planner): faster obstacle avoidance planner tier4/autoware.universe#743

Merged

7 tasks

takayuki5168 mentioned this pull request Nov 30, 2023

feat(obstacle_avoidance_planner): faster optimization by sparser re-formulation (#4600) tier4/autoware.universe#1051

Merged

4 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(obstacle_avoidance_planner): faster optimization by sparser re-formulation #4600

feat(obstacle_avoidance_planner): faster optimization by sparser re-formulation #4600

takayuki5168 commented Aug 11, 2023 •

edited

Loading

takayuki5168 Aug 12, 2023

codecov bot commented Aug 12, 2023 •

edited

Loading

feat(obstacle_avoidance_planner): faster optimization by sparser re-formulation #4600

feat(obstacle_avoidance_planner): faster optimization by sparser re-formulation #4600

Conversation

takayuki5168 commented Aug 11, 2023 • edited Loading

Description

Qualitative evaluation

Tests performed

Effects on system behavior

Pre-review checklist for the PR author

In-review checklist for the PR reviewers

Post-review checklist for the PR author

takayuki5168 Aug 12, 2023

Choose a reason for hiding this comment

codecov bot commented Aug 12, 2023 • edited Loading

Codecov Report

takayuki5168 commented Aug 11, 2023 •

edited

Loading

codecov bot commented Aug 12, 2023 •

edited

Loading