Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(osqp_interface): add interface to give csc matrix directly to enable warm start for computation time improvement. #346

Closed
wants to merge 4 commits into from

Conversation

takayuki5168
Copy link
Contributor

@takayuki5168 takayuki5168 commented Feb 7, 2022

Related Issue(required)

#347

Description(required)

Add a new interface in osqp_interface to enable warm-start which improves the computation time of the optimization. Details are below.

  • added initializer of osqp_interface with csc matrix, and matrix update functions with csc matrix
  • added a function to validate the size of P, q, A, l, u.
  • fixed the existing test and added test for new functions

NOTE: we do not have to care about the existing applications which use osqp_interface since the existing member functions' behavior of osqp_interface will not change.

Review Procedure(required)

run test

Related PR(optional)

Pre-Review Checklist for the PR Author

PR Author should check the checkboxes below when creating the PR.

If you are adding new package following items are required:

  • Documentation with description of the package is available
  • A sample launch file and parameter file are available if the package contains executable nodes

Checklist for the PR Reviewer

Reviewers should check the checkboxes below before approval.

  • Commits are properly organized and messages are according to the guideline
  • PR title describes the changes

Post-Review Checklist for the PR Author

PR Author should check the checkboxes below before merging.

  • All open points are addressed and tracked via issues or tickets

CI Checks

  • Build and test for PR / build-and-test-pr: Required to pass before the merge.
  • Build and test for PR / clang-tidy-pr: NOT required to pass before the merge. It is up to the reviewer(s). Found false positives? See the [guidelines][clang-tidy-guidelines].
  • Check spelling: NOT required to pass before the merge. It is up to the reviewer(s). See here if you want to add some words to the spell check dictionary.

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
@takayuki5168 takayuki5168 force-pushed the feature/osqp-add-functions branch from 6b22389 to f108d12 Compare February 7, 2022 02:54
@@ -57,7 +57,7 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
int64_t m_exitflag;

// Runs the solver on the stored problem.
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t> solve();
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t> solve();
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

add iteration, which I guess an important value to the application

Comment on lines +207 to +233
// check if arguments are valid
std::stringstream ss;
if (P.rows() != P.cols()) {
ss << "P.rows() and P.cols() are not the same. P.rows() = " << P.rows()
<< ", P.cols() = " << P.cols();
throw std::invalid_argument(ss.str());
}
if (P.rows() != static_cast<int>(q.size())) {
ss << "P.rows() and q.size() are not the same. P.rows() = " << P.rows()
<< ", q.size() = " << q.size();
throw std::invalid_argument(ss.str());
}
if (P.rows() != A.cols()) {
ss << "P.rows() and A.cols() are not the same. P.rows() = " << P.rows()
<< ", A.cols() = " << A.cols();
throw std::invalid_argument(ss.str());
}
if (A.rows() != static_cast<int>(l.size())) {
ss << "A.rows() and l.size() are not the same. A.rows() = " << A.rows()
<< ", l.size() = " << l.size();
throw std::invalid_argument(ss.str());
}
if (A.rows() != static_cast<int>(u.size())) {
ss << "A.rows() and u.size() are not the same. A.rows() = " << A.rows()
<< ", u.size() = " << u.size();
throw std::invalid_argument(ss.str());
}
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

check if rols and cols of matrix, vector among P, q, A u, l have no conflict. (Once this checking function was in autoware, but dropped during .universe porting.)

Comment on lines +299 to +300
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t> result =
std::make_tuple(sol_primal, sol_lagrange_multiplier, status_polish, status_solution, status_iteration);
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

(add iteration)

};

{
// Define problem during optimization
autoware::common::osqp::OSQPInterface osqp;
Eigen::MatrixXd P(2, 2);
P << 4, 1, 1, 2;
Eigen::MatrixXd A(2, 4);
Eigen::MatrixXd A(4, 2);
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

"A" matrix size was wrong.

EXPECT_EQ(std::get<2>(result), 1);
EXPECT_EQ(std::get<3>(result), 1);
ASSERT_EQ(std::get<0>(result).size(), size_t(2));
ASSERT_EQ(std::get<1>(result).size(), size_t(2));
EXPECT_DOUBLE_EQ(std::get<0>(result)[0], 0.3);
EXPECT_DOUBLE_EQ(std::get<0>(result)[1], 0.7);
EXPECT_DOUBLE_EQ(std::get<1>(result)[0], -2.9);
EXPECT_NEAR(std::get<1>(result)[1], 0.2, 1e-6);
EXPECT_NEAR(std::get<1>(result)[1], 0.0, 1e-6);
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Without this change, the test failed, which I'm not sure why now.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@rej55 Do you have any idea about this change (from 0.2 to 0.0)? For the code, I just changed from Eigen::MatrixXd A(2, 4); to Eigen::MatrixXd A(4, 2); as following diffs on this page.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@takayuki5168
Since the problem has changed, I think it is proper to change the solution.
However, I wonder why the previous code was working.... 🤔

In this problem, the size of Lagrange multiplier should be 4 because the row size of the constraint matrix A is 4.
However, in this test code, it expects the size of Lagrange multiplier as 2.
Could you tell me the size of the resultant Lagrange multiplier?

ASSERT_EQ(std::get<1>(result).size(), size_t(2));

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@rej55
The size of lagrange multiplier does not change, and it is 2. Not 4 as you pointed 🤔

ASSERT_EQ(std::get<1>(result).size(), size_t(2));

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Seems this problem occurs like below:

  • CSC matrix does not check the dimension consistency.
  • Then, the A(2, 4) matrix was considered as A(2, 2) and the half of the A matrix was ignored in the optimization.
  • But the optimal solution of the prime problem was accidentally unchanged. This is because the optimal solution does not on the ignored constraints.
  • The dual variable had a different dimension from the original solution, but this was not checked in the test.

So this will be solved by adding the full evaluation of the dual variables.

Copy link
Contributor

@TakaHoribe TakaHoribe Feb 13, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@codecov
Copy link

codecov bot commented Feb 7, 2022

Codecov Report

Merging #346 (a694806) into tier4/proposal (312753a) will increase coverage by 9.59%.
The diff coverage is 14.08%.

Impacted file tree graph

@@                Coverage Diff                 @@
##           tier4/proposal     #346      +/-   ##
==================================================
+ Coverage            9.33%   18.92%   +9.59%     
==================================================
  Files                 701       68     -633     
  Lines               48726     8485   -40241     
  Branches             5807     1967    -3840     
==================================================
- Hits                 4549     1606    -2943     
+ Misses              40366     5196   -35170     
+ Partials             3811     1683    -2128     
Impacted Files Coverage Δ
...ity_smoother/src/motion_velocity_smoother_node.cpp 0.00% <0.00%> (ø)
common/osqp_interface/test/test_osqp_interface.cpp 5.63% <2.70%> (-3.90%) ⬇️
common/osqp_interface/src/osqp_interface.cpp 35.03% <27.27%> (-2.18%) ⬇️
...usion_spot/scene_occlusion_spot_in_public_road.hpp
...ocessor/include/pointcloud_preprocessor/filter.hpp
...mon/autoware_auto_common/test/test_angle_utils.cpp
...em_error_monitor/src/system_error_monitor_core.cpp
...rc/livox_tag_filter_node/livox_tag_filter_node.cpp
...ion/include/fault_injection/diagnostic_storage.hpp
...y_planner/src/scene_module/traffic_light/debug.cpp
... and 627 more

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 5465e38...a694806. Read the comment docs.

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
@takayuki5168 takayuki5168 marked this pull request as ready for review February 7, 2022 03:25
@takayuki5168
Copy link
Contributor Author

takayuki5168 commented Feb 7, 2022

@rej55 @tkimura4 @TakaHoribe This is ready for review. 🙏 (This is a part of obstacle_avoidance_planner improvement)

@takayuki5168 takayuki5168 requested a review from tkimura4 February 7, 2022 10:33
@takayuki5168 takayuki5168 changed the title feat: osqp add functions feat(osqp_interface): add functions to give csc matrix directly to osqp_interface Feb 8, 2022
@TakaHoribe TakaHoribe changed the title feat(osqp_interface): add functions to give csc matrix directly to osqp_interface feat(osqp_interface): add interface to give csc matrix directly to enable warm start. Feb 12, 2022
@TakaHoribe TakaHoribe changed the title feat(osqp_interface): add interface to give csc matrix directly to enable warm start. feat(osqp_interface): add interface to give csc matrix directly to enable warm start for computation time improvement. Feb 12, 2022
@takayuki5168
Copy link
Contributor Author

replaced with #378

@takayuki5168 takayuki5168 deleted the feature/osqp-add-functions branch February 14, 2022 01:58
deepmeng pushed a commit to collector-m/autoware.universe that referenced this pull request Feb 18, 2022
* release v0.4.0

* Modify to use projection matrix with undistorted 2D result (autowarefoundation#722)

Signed-off-by: Akihito Ohasto <aohsato@gmail.com>

* remove ROS1 packages temporarily

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Revert "remove ROS1 packages temporarily"

This reverts commit 6e1593f7d630fc1e670df40fe1723890fc238f17.

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* add COLCON_IGNORE to ros1 packages

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Rename launch files to launch.xml (autowarefoundation#28)

* Rename h files to hpp (autowarefoundation#142)

* Change includes

* Rename files

* Adjustments to make things compile

* Other packages

* Adjust copyright notice on 532 out of 699 source files (autowarefoundation#143)

* Use quotes for includes where appropriate (autowarefoundation#144)

* Use quotes for includes where appropriate

* Fix lint tests

* Make tests pass hopefully

* ported roi_cluster_fusion from ROS1 to ROS2 (autowarefoundation#118)

compiles succesfully.
Getting some undefined reference error while launching the node.

* Port euclidean cluster (autowarefoundation#120)

* porting CmakeLists.txt and package.xml in progress

* ported CMakeLists.txt and package.xml to ROS2

* Ported euclidean_cluster from ROS1 to ROS2

* deleted unnecesary files

* fixed transient_local

Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* hotfix roi_cluster_fusion (autowarefoundation#239)

* Fix perception launches (autowarefoundation#240)

* fix roi_cluster_fusion launch

Signed-off-by: Takamasa Horibe <takamasa.horib@gmail.com>

* add comment on launch

Signed-off-by: Takamasa Horibe <takamasa.horib@gmail.com>

Co-authored-by: Takamasa Horibe <takamasa.horib@gmail.com>

* Ros2 v0.8.0 roi cluster fusion (autowarefoundation#321)

* restore v0.5.0

* Feature/fusion debug (autowarefoundation#1051)

* add debuger

* change param

* add publisher

* Compatible with opencv4 (autowarefoundation#1156)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* refactor for ros2 style

* fix code

* to hpp

* fix code

* uncursify

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>

* [roi_cluster_fusion]: Avoid redeclaring parameter for compressed image publisher (autowarefoundation#346)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix transform (autowarefoundation#420)

* Replace rclcpp::Time(0) by tf2::TimePointZero() in lookupTransform

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix canTransform

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix test

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix typo in perception module (autowarefoundation#440)

* add use_sim-time option (autowarefoundation#454)

* Format launch files (autowarefoundation#1219)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Unify Apache-2.0 license name (autowarefoundation#1242)

* Remove use_sim_time for set_parameter (autowarefoundation#1260)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Add pre-commit (autowarefoundation#1560)

* add pre-commit

* add pre-commit-config

* add additional settings for private repository

* use default pre-commit-config

* update pre-commit setting

* Ignore whitespace for line breaks in markdown

* Update .github/workflows/pre-commit.yml

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

* exclude svg

* remove pretty-format-json

* add double-quote-string-fixer

* consider COLCON_IGNORE file when seaching modified package

* format file

* pre-commit fixes

* Update pre-commit.yml

* Update .pre-commit-config.yaml

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: pre-commit <pre-commit@example.com>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

* Fix -Wunused-parameter (autowarefoundation#1836)

* Fix -Wunused-parameter

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix mistake

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* fix spell

* Fix lint issues

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Ignore flake8 warnings

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>

* add sort-package-xml hook in pre-commit (autowarefoundation#1881)

* add sort xml hook in pre-commit

* change retval to exit_status

* rename

* add prettier plugin-xml

* use early return

* add license note

* add tier4 license

* restore prettier

* change license order

* move local hooks to public repo

* move prettier-xml to pre-commit-hooks-ros

* update version for bug-fix

* apply pre-commit

* [roi_cluster_fusion]: Fix poting mistake (autowarefoundation#1624)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* fix typo & check debugger (autowarefoundation#1511) (autowarefoundation#1625)

Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com>

* Use set_remap for if tag (autowarefoundation#1800)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Apply format (autowarefoundation#1999)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

Fix cpplint

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Detection by tracker (autowarefoundation#1910)

* initial commit

* backup

* apply format

* cosmetic change

* implement divided under segmenterd clusters

* cosmetic change

* bug fix

* bug fix

* bug fix

* modify launch

* add debug and bug fix

* bug fix

* bug fix

* add no found tracked object

* modify parameters and cmake

* bug fix

* remove debug info

* add readme

* modify clustering launch

* run pre-commit

* cosmetic change

* cosmetic change

* cosmetic change

* apply markdownlint

* modify launch

* modify for cpplint

* modify qos

* change int to size_T

* bug fix

* change perception qos

* Update perception/object_recognition/detection/detection_by_tracker/package.xml

Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>

* cosmetic change

* cosmetic change

* fix launch

* Update perception/object_recognition/detection/detection_by_tracker/src/utils.cpp

Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>

* modify header include order

* change include order

* Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp

Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>

* change to std::optional

* cosmetic change

* Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp

Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>

* Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp

Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>

* bug fix

* modify readme

Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>

* sync rc rc/v0.23.0 (autowarefoundation#2219)

* Fix qos in roi cluster fusion (autowarefoundation#2218)

* fix confidence (autowarefoundation#2220)

Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

* Change formatter to clang-format and black (autowarefoundation#2332)

* Revert "Temporarily comment out pre-commit hooks"

This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3.

* Replace ament_lint_common with autoware_lint_common

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Remove ament_cmake_uncrustify and ament_clang_format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply Black

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix for cpplint

* Fix include double quotes to angle brackets

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add COLCON_IGNORE (autowarefoundation#500)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* port roi cluster fusion (autowarefoundation#536)

* remove COLCON_IGNORE

* use DetectedObjectsWithFeature

* fix topic type

* update overwrite condition

* add README of roi_cluster_fusion (autowarefoundation#653)

* add README of roi_cluster_fusion

* fix typo

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

* add image

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>
Co-authored-by: Akihito Ohsato <aohsato@gmail.com>
Co-authored-by: Nikolai Morin <nnmmgit@gmail.com>
Co-authored-by: nik-tier4 <71747268+nik-tier4@users.noreply.github.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: Takamasa Horibe <takamasa.horib@gmail.com>
Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com>
Co-authored-by: pre-commit <pre-commit@example.com>
Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>
Co-authored-by: autoware-iv-sync-ci[bot] <87871706+autoware-iv-sync-ci[bot]@users.noreply.github.com>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com>
TomohitoAndo pushed a commit to TomohitoAndo/autoware.universe that referenced this pull request Oct 5, 2022
Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>
KYabuuchi pushed a commit to KYabuuchi/autoware.universe that referenced this pull request Feb 16, 2023
* feat(docker image): add no-CUDA images

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* fix ref

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>
keiota pushed a commit to keiota/autoware.universe that referenced this pull request Aug 17, 2023
…ndation#346)

Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants