Skip to content

Commit 758f33c

Browse files
NorahXiongpre-commit-ci[bot]youtalk
authored
test(autoware_qp_interface): add unit tests for initializeProblem (#237)
* test(autoware_qp_interface): add unit tests for QPInterface::initializeProblem Signed-off-by: NorahXiong <norah.xiong@autocore.ai> * test(autoware_qp_interface): add unit tests for class OSQPInterface Signed-off-by: NorahXiong <norah.xiong@autocore.ai> * test(autoware_qp_interface): add unit tests for class ProxQPInterface Signed-off-by: NorahXiong <norah.xiong@autocore.ai> * style(pre-commit): autofix --------- Signed-off-by: NorahXiong <norah.xiong@autocore.ai> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>
1 parent b1e7308 commit 758f33c

File tree

4 files changed

+237
-0
lines changed

4 files changed

+237
-0
lines changed

Diff for: ‎common/autoware_qp_interface/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -54,6 +54,7 @@ if(BUILD_TESTING)
5454
test/test_osqp_interface.cpp
5555
test/test_csc_matrix_conv.cpp
5656
test/test_proxqp_interface.cpp
57+
test/test_qp_interface.cpp
5758
)
5859
set(TEST_OSQP_INTERFACE_EXE test_osqp_interface)
5960
ament_add_ros_isolated_gtest(${TEST_OSQP_INTERFACE_EXE} ${TEST_SOURCES})

Diff for: ‎common/autoware_qp_interface/test/test_osqp_interface.cpp

+92
Original file line numberDiff line numberDiff line change
@@ -167,4 +167,96 @@ TEST(TestOsqpInterface, BasicQp)
167167
// EXPECT_EQ(osqp.getTakenIter(), 1);
168168
}
169169
}
170+
171+
TEST(TestOsqpInterface, UpdateSettingsBeforeInitialization)
172+
{
173+
using autoware::qp_interface::calCSCMatrix;
174+
using autoware::qp_interface::calCSCMatrixTrapezoidal;
175+
using autoware::qp_interface::CSC_Matrix;
176+
177+
const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished();
178+
const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished();
179+
const std::vector<double> q = {1.0, 1.0};
180+
const std::vector<double> l = {1.0, 0.0, 0.0, -autoware::qp_interface::OSQP_INF};
181+
const std::vector<double> u = {1.0, 0.7, 0.7, autoware::qp_interface::OSQP_INF};
182+
183+
// Test update settings before initialization
184+
autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6);
185+
186+
osqp.updateEpsAbs(1.0e-4);
187+
osqp.updateEpsRel(1.0e-4);
188+
osqp.updateMaxIter(1000);
189+
osqp.updateVerbose(false);
190+
osqp.updateRhoInterval(25);
191+
osqp.updateRho(1.6);
192+
osqp.updateAlpha(1.0);
193+
osqp.updateScaling(5);
194+
osqp.updatePolish(true);
195+
osqp.updatePolishRefinementIteration(5);
196+
osqp.updateCheckTermination(10);
197+
198+
const auto solution = osqp.optimize(calCSCMatrixTrapezoidal(P), calCSCMatrix(A), q, l, u);
199+
const auto status = osqp.getStatus();
200+
const auto polish_status = osqp.getPolishStatus();
201+
202+
EXPECT_EQ(status, "OSQP_SOLVED");
203+
EXPECT_EQ(polish_status, 1);
204+
205+
static const auto ep = 1.0e-8;
206+
207+
ASSERT_EQ(solution.size(), size_t(2));
208+
EXPECT_NEAR(solution[0], 0.3, ep);
209+
EXPECT_NEAR(solution[1], 0.7, ep);
210+
}
211+
212+
TEST(TestOsqpInterface, UpdateSettingsAfterInitialization)
213+
{
214+
// Test update settings after initialization
215+
using autoware::qp_interface::calCSCMatrix;
216+
using autoware::qp_interface::calCSCMatrixTrapezoidal;
217+
using autoware::qp_interface::CSC_Matrix;
218+
219+
const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished();
220+
const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished();
221+
const std::vector<double> q = {1.0, 1.0};
222+
const std::vector<double> l = {1.0, 0.0, 0.0, -autoware::qp_interface::OSQP_INF};
223+
const std::vector<double> u = {1.0, 0.7, 0.7, autoware::qp_interface::OSQP_INF};
224+
225+
// Dummy initial problem with csc matrix
226+
CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2));
227+
CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2));
228+
std::vector<double> q_ini(2, 0.0);
229+
std::vector<double> l_ini(4, 0.0);
230+
std::vector<double> u_ini(4, 0.0);
231+
autoware::qp_interface::OSQPInterface osqp(true, 4000, 1e-6); // enable warm start
232+
osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini);
233+
234+
CSC_Matrix P_csc_new = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Ones(2, 2));
235+
CSC_Matrix A_csc_new = calCSCMatrix(Eigen::MatrixXd::Ones(4, 2));
236+
237+
osqp.updateEpsAbs(1.0e-4);
238+
osqp.updateEpsRel(1.0e-4);
239+
osqp.updateMaxIter(1000);
240+
osqp.updateVerbose(false);
241+
osqp.updateRhoInterval(25);
242+
osqp.updateRho(1.6);
243+
osqp.updateAlpha(1.0);
244+
osqp.updateScaling(5);
245+
osqp.updatePolish(true);
246+
osqp.updatePolishRefinementIteration(5);
247+
osqp.updateCheckTermination(10);
248+
osqp.updateP((Eigen::MatrixXd(2, 2) << 8, 2, 2, 4).finished());
249+
osqp.updateQ(std::vector<double>(2, 1.0));
250+
osqp.updateL(std::vector<double>(2, 1.0));
251+
osqp.updateU(std::vector<double>(2, 1.0));
252+
osqp.updateBounds(std::vector<double>(2, 1.0), std::vector<double>(2, 1.0));
253+
osqp.updateCscP(P_csc_new);
254+
osqp.updateCscA(A_csc_new);
255+
256+
const auto solution = osqp.optimize(P_csc_new, A_csc_new, q, l, u);
257+
const auto status = osqp.getStatus();
258+
259+
EXPECT_EQ(status, "OSQP_SOLVED");
260+
}
261+
170262
} // namespace

Diff for: ‎common/autoware_qp_interface/test/test_proxqp_interface.cpp

+9
Original file line numberDiff line numberDiff line change
@@ -80,6 +80,15 @@ TEST(TestProxqpInterface, BasicQp)
8080
check_result(solution, status);
8181
EXPECT_EQ(proxqp.getIterationNumber(), 0);
8282
}
83+
{
84+
proxqp.updateEpsAbs(1.0e-4);
85+
proxqp.updateEpsRel(1.0e-4);
86+
proxqp.updateVerbose(true);
87+
const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u);
88+
const auto status = proxqp.getStatus();
89+
check_result(solution, status);
90+
EXPECT_EQ(proxqp.getIterationNumber(), 0);
91+
}
8392
}
8493
}
8594
} // namespace

Diff for: ‎common/autoware_qp_interface/test/test_qp_interface.cpp

+135
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,135 @@
1+
// Copyright 2023 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "autoware/qp_interface/osqp_interface.hpp"
16+
#include "autoware/qp_interface/qp_interface.hpp"
17+
18+
#include <Eigen/Dense>
19+
20+
#include <gtest/gtest.h>
21+
22+
#include <stdexcept>
23+
#include <vector>
24+
25+
namespace autoware::qp_interface
26+
{
27+
TEST(QPInterfaceTest, InitializeProblem_NonSquareP_ThrowsException)
28+
{
29+
Eigen::MatrixXd P(2, 3);
30+
Eigen::MatrixXd A(1, 2);
31+
std::vector<double> q = {1.0, 2.0};
32+
std::vector<double> l = {1.0};
33+
std::vector<double> u = {1.0};
34+
bool enable_warm_start = false;
35+
c_float eps_abs = 1e-4;
36+
37+
EXPECT_THROW(
38+
{ OSQPInterface osqp_instance(P, A, q, l, u, enable_warm_start, eps_abs); },
39+
std::invalid_argument);
40+
}
41+
42+
TEST(QPInterfaceTest, InitializeProblem_PRowsNotEqualQSize_ThrowsException)
43+
{
44+
Eigen::MatrixXd P(2, 2);
45+
Eigen::MatrixXd A(1, 2);
46+
std::vector<double> q = {1.0};
47+
std::vector<double> l = {1.0};
48+
std::vector<double> u = {1.0};
49+
bool enable_warm_start = false;
50+
c_float eps_abs = 1e-4;
51+
52+
EXPECT_THROW(
53+
{ OSQPInterface osqp_instance(P, A, q, l, u, enable_warm_start, eps_abs); },
54+
std::invalid_argument);
55+
}
56+
57+
TEST(QPInterfaceTest, InitializeProblem_PRowsNotEqualACols_ThrowsException)
58+
{
59+
Eigen::MatrixXd P(2, 2);
60+
Eigen::MatrixXd A(1, 3);
61+
std::vector<double> q = {1.0, 2.0};
62+
std::vector<double> l = {1.0};
63+
std::vector<double> u = {1.0};
64+
bool enable_warm_start = false;
65+
c_float eps_abs = 1e-4;
66+
67+
EXPECT_THROW(
68+
{ OSQPInterface osqp_instance(P, A, q, l, u, enable_warm_start, eps_abs); },
69+
std::invalid_argument);
70+
}
71+
72+
TEST(QPInterfaceTest, InitializeProblem_ARowsNotEqualLSize_ThrowsException)
73+
{
74+
Eigen::MatrixXd P(2, 2);
75+
Eigen::MatrixXd A(2, 2);
76+
std::vector<double> q = {1.0, 2.0};
77+
std::vector<double> l = {1.0};
78+
std::vector<double> u = {1.0, 2.0};
79+
bool enable_warm_start = false;
80+
c_float eps_abs = 1e-4;
81+
82+
EXPECT_THROW(
83+
{ OSQPInterface osqp_instance(P, A, q, l, u, enable_warm_start, eps_abs); },
84+
std::invalid_argument);
85+
}
86+
87+
TEST(QPInterfaceTest, InitializeProblem_ARowsNotEqualUSize_ThrowsException)
88+
{
89+
Eigen::MatrixXd P(2, 2);
90+
Eigen::MatrixXd A(2, 2);
91+
std::vector<double> q = {1.0, 2.0};
92+
std::vector<double> l = {1.0, 2.0};
93+
std::vector<double> u = {1.0};
94+
bool enable_warm_start = false;
95+
c_float eps_abs = 1e-4;
96+
97+
EXPECT_THROW(
98+
{ OSQPInterface osqp_instance(P, A, q, l, u, enable_warm_start, eps_abs); },
99+
std::invalid_argument);
100+
}
101+
102+
TEST(QPInterfaceTest, InitializeProblem_ValidInputs_Success)
103+
{
104+
Eigen::MatrixXd P(2, 2);
105+
P << 1, 0, 0, 1;
106+
Eigen::MatrixXd A(1, 2);
107+
A << 1, 1;
108+
std::vector<double> q = {1.0, 2.0};
109+
std::vector<double> l = {1.0};
110+
std::vector<double> u = {2.0};
111+
bool enable_warm_start = false;
112+
c_float eps_abs = 1e-4;
113+
114+
OSQPInterface osqp_instance(P, A, q, l, u, enable_warm_start, eps_abs);
115+
EXPECT_NO_THROW({ OSQPInterface osqp_instance(P, A, q, l, u, enable_warm_start, eps_abs); });
116+
}
117+
118+
TEST(QPInterfaceTest, Optimize_ValidInputs_ReturnsResult)
119+
{
120+
Eigen::MatrixXd P(2, 2);
121+
P << 1, 0, 0, 1;
122+
Eigen::MatrixXd A(1, 2);
123+
A << 1, 1;
124+
std::vector<double> q = {1.0, 2.0};
125+
std::vector<double> l = {1.0};
126+
std::vector<double> u = {1.0};
127+
bool enable_warm_start = false;
128+
c_float eps_abs = 1e-4;
129+
130+
OSQPInterface osqp(P, A, q, l, u, enable_warm_start, eps_abs);
131+
std::vector<double> result = osqp.QPInterface::optimize(P, A, q, l, u);
132+
EXPECT_EQ(result.size(), 2);
133+
}
134+
135+
} // namespace autoware::qp_interface

0 commit comments

Comments
 (0)