From 80b3f23f35f36cc2f3be1aaaddd8569bb4330e59 Mon Sep 17 00:00:00 2001 From: Maxim Skripnik Date: Mon, 15 Jan 2024 09:58:22 +0100 Subject: [PATCH 1/3] fix #245: Cannot convert from initializer list to toppra::BoundaryCond --- cpp/tests/test_cubic_spline.cpp | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/cpp/tests/test_cubic_spline.cpp b/cpp/tests/test_cubic_spline.cpp index 19326ed0..d63f34b6 100644 --- a/cpp/tests/test_cubic_spline.cpp +++ b/cpp/tests/test_cubic_spline.cpp @@ -76,7 +76,7 @@ Vectors CubicSpline::positions; Vector CubicSpline::times; TEST_F(CubicSpline, ClampedSpline) { - BoundaryCond bc{1, {0, 0, 0, 0, 0, 0}}; + BoundaryCond bc{1, std::vector{0, 0, 0, 0, 0, 0}}; BoundaryCondFull bc_type{bc, bc}; ConstructCubicSpline(positions, times, bc_type); AssertSplineKnots(); @@ -92,7 +92,7 @@ TEST_F(CubicSpline, ClampedSplineString) { } TEST_F(CubicSpline, NaturalSpline) { - BoundaryCond bc{2, {0, 0, 0, 0, 0, 0}}; + BoundaryCond bc{2, std::vector{0, 0, 0, 0, 0, 0}}; BoundaryCondFull bc_type{bc, bc}; ConstructCubicSpline(positions, times, bc_type); AssertSplineKnots(); @@ -100,8 +100,8 @@ TEST_F(CubicSpline, NaturalSpline) { } TEST_F(CubicSpline, FirstOrderBoundaryConditions) { - BoundaryCondFull bc_type{BoundaryCond{1, {1.25, 0, 4.12, 1.75, 7.43, 5.31}}, - BoundaryCond{1, {3.51, 5.32, 4.63, 0, -3.12, 3.53}}}; + BoundaryCondFull bc_type{BoundaryCond{1, std::vector{1.25, 0, 4.12, 1.75, 7.43, 5.31}}, + BoundaryCond{1, std::vector{3.51, 5.32, 4.63, 0, -3.12, 3.53}}}; ConstructCubicSpline(positions, times, bc_type); AssertSplineKnots(); AssertSplineBoundaryConditions(bc_type); @@ -109,8 +109,8 @@ TEST_F(CubicSpline, FirstOrderBoundaryConditions) { TEST_F(CubicSpline, SecondOrderBoundaryConditions) { BoundaryCondFull bc_type{ - BoundaryCond{2, {1.52, -4.21, 7.21, 9.31, -1.53, 7.54}}, - BoundaryCond{2, {-5.12, 8.21, 9.12, 5.12, 24.12, 9.42}}}; + BoundaryCond{2, std::vector{1.52, -4.21, 7.21, 9.31, -1.53, 7.54}}, + BoundaryCond{2, std::vector{-5.12, 8.21, 9.12, 5.12, 24.12, 9.42}}}; ConstructCubicSpline(positions, times, bc_type); AssertSplineKnots(); AssertSplineBoundaryConditions(bc_type); @@ -118,8 +118,8 @@ TEST_F(CubicSpline, SecondOrderBoundaryConditions) { TEST_F(CubicSpline, FirstOrderAndSecondOrderBoundaryConditions) { BoundaryCondFull bc_type{ - BoundaryCond{1, {1.52, -4.21, 7.21, 9.31, -1.53, 7.54}}, - BoundaryCond{2, {-5.12, 8.21, 9.12, 5.12, 24.12, 9.42}}}; + BoundaryCond{1, std::vector{1.52, -4.21, 7.21, 9.31, -1.53, 7.54}}, + BoundaryCond{2, std::vector{-5.12, 8.21, 9.12, 5.12, 24.12, 9.42}}}; ConstructCubicSpline(positions, times, bc_type); AssertSplineKnots(); AssertSplineBoundaryConditions(bc_type); @@ -132,8 +132,8 @@ TEST_F(CubicSpline, BadPositions) { {6.25, 8.12, 9.52, 5.21, 8.31}, {7.31, 3.53, 8.41, 9.56, -3.15, 4.83}}); BoundaryCondFull bc_type{ - BoundaryCond{2, {1.52, -4.21, 7.21, 9.31, -1.53, 7.54}}, - BoundaryCond{2, {-5.12, 8.21, 9.12, 5.12, 24.12, 9.42}}}; + BoundaryCond{2, std::vector{1.52, -4.21, 7.21, 9.31, -1.53, 7.54}}, + BoundaryCond{2, std::vector{-5.12, 8.21, 9.12, 5.12, 24.12, 9.42}}}; ASSERT_THROW(ConstructCubicSpline(bad_positions, times, bc_type), std::runtime_error); @@ -147,8 +147,8 @@ TEST_F(CubicSpline, BadPositions) { TEST_F(CubicSpline, BadTimes) { BoundaryCondFull bc_type{ - BoundaryCond{2, {1.52, -4.21, 7.21, 9.31, -1.53, 7.54}}, - BoundaryCond{2, {-5.12, 8.21, 9.12, 5.12, 24.12, 9.42}}}; + BoundaryCond{2, std::vector{1.52, -4.21, 7.21, 9.31, -1.53, 7.54}}, + BoundaryCond{2, std::vector{-5.12, 8.21, 9.12, 5.12, 24.12, 9.42}}}; Vector bad_times(1); bad_times << 1; ASSERT_THROW(ConstructCubicSpline(positions, bad_times, bc_type), @@ -167,8 +167,8 @@ TEST_F(CubicSpline, BadTimes) { TEST_F(CubicSpline, BadBoundaryConditions) { BoundaryCondFull bc_type{ - BoundaryCond{2, {1.52, 7.21, 9.31, -1.53, 7.54}}, - BoundaryCond{2, {-5.12, 8.21, 9.12, 5.12, 24.12, 9.42}}}; + BoundaryCond{2, std::vector{1.52, 7.21, 9.31, -1.53, 7.54}}, + BoundaryCond{2, std::vector{-5.12, 8.21, 9.12, 5.12, 24.12, 9.42}}}; ASSERT_THROW(ConstructCubicSpline(positions, times, bc_type), std::runtime_error); @@ -177,8 +177,8 @@ TEST_F(CubicSpline, BadBoundaryConditions) { ASSERT_THROW(ConstructCubicSpline(positions, times, bc_type), std::runtime_error); - bc_type = {BoundaryCond{3, {1.52, 7.21, 9.31, 2.52, 4.41, 5.54}}, - BoundaryCond{3, {-5.12, 8.21, 9.12, -1.32, 3.53, 9.21}}}; + bc_type = {BoundaryCond{3, std::vector{1.52, 7.21, 9.31, 2.52, 4.41, 5.54}}, + BoundaryCond{3, std::vector{-5.12, 8.21, 9.12, -1.32, 3.53, 9.21}}}; ASSERT_THROW(ConstructCubicSpline(positions, times, bc_type), std::runtime_error); } From b47400c38d54b4655fef13c70a97670f2e243dbe Mon Sep 17 00:00:00 2001 From: Maxim Skripnik Date: Mon, 15 Jan 2024 14:14:18 +0100 Subject: [PATCH 2/3] fix #244: Seidel LP 1D: incoherent bounds --- cpp/src/toppra/solver/seidel-internal.hpp | 8 ++++++++ cpp/tests/test_solver.cpp | 20 +++++++++++++++++++- 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/cpp/src/toppra/solver/seidel-internal.hpp b/cpp/src/toppra/solver/seidel-internal.hpp index 22afdb8e..362d880b 100644 --- a/cpp/src/toppra/solver/seidel-internal.hpp +++ b/cpp/src/toppra/solver/seidel-internal.hpp @@ -116,6 +116,14 @@ LpSol1d solve_lp1d(const RowVector2& v, const Eigen::MatrixBase& A) } } + // In case upper bound becomes less than lower bound and their difference is not more than + // 2*ABS_TOLERANCE, extend both bounds to not make the problem infeasible due to numerical + // errors. + if (cur_max < cur_min && cur_min - cur_max < 2 * ABS_TOLERANCE) { + cur_min -= ABS_TOLERANCE; + cur_max += ABS_TOLERANCE; + } + if ( cur_min - cur_max > std::max(std::abs(cur_min),std::abs(cur_max))*REL_TOLERANCE || cur_min == infinity || cur_max == -infinity) { diff --git a/cpp/tests/test_solver.cpp b/cpp/tests/test_solver.cpp index dde35163..70a2b408 100644 --- a/cpp/tests/test_solver.cpp +++ b/cpp/tests/test_solver.cpp @@ -288,7 +288,6 @@ TEST(SeidelFunctions_1D, infeasible_due_to_incoherent_bounds) { A << seidel::TINY / 2, 1; sol = seidel::solve_lp1d(v, A); EXPECT_FALSE(sol.feasible); - } TEST(SeidelFunctions_1D, feasible_with_a_and_b_small) { @@ -346,6 +345,25 @@ TEST(SeidelFunctions_1D, feasible_but_with_tolerated_constraint_violation) { auto sol = seidel::solve_lp1d(v, A); EXPECT_TRUE(sol.feasible); + + { + // Issue #244 + v = { -65.960772491990838, 0.0 }; + A.resize(8, 2); + A << + -65.9607724919908, -0.0151605259038078, + 65.9607724919908, -0.00782362866419491, + 0.468679477393087, 5.6843418860808e-14, + -0.468679477393087, -1000, + 0, -std::numeric_limits::infinity(), + 0, -std::numeric_limits::infinity(), + -65.9607724919908, 0, + 65.9607724919908, -0.0229841545680027; + + auto sol = seidel::solve_lp1d(v, A); + EXPECT_TRUE(sol.feasible); + EXPECT_NEAR(0, sol.optvar, TOPPRA_ABS_TOL); + } } TEST(SeidelFunctions, seidel_2d) { From b7b76d8418baddc0f6a4c2b43c187128d63cf6bf Mon Sep 17 00:00:00 2001 From: Maxim Skripnik Date: Mon, 15 Jan 2024 14:21:34 +0100 Subject: [PATCH 3/3] Update HISTORY.md --- HISTORY.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/HISTORY.md b/HISTORY.md index 0168f879..3b056dd1 100644 --- a/HISTORY.md +++ b/HISTORY.md @@ -1,5 +1,8 @@ # Changelog +- [cpp]: fix: Seidel LP 1D: incoherent bounds (#244) +- [cpp]: fix: Cannot convert from 'initializer list' to 'toppra::BoundaryCond' (#245) + ## 0.6.2 (Sept 19 2023) ### Changed