add benchmark, numeric stability

CMakeLists.txt

@@ -8,6 +8,7 @@ list(INSERT CMAKE_MODULE_PATH 0 ${CMAKE_SOURCE_DIR}/cmake)
 option(BUILD_EXAMPLES "Build example programs" OFF)
 option(BUILD_PYTHON_MODULE "Build python module" ON)
 option(BUILD_TESTS "Build tests" ON)
+option(BUILD_BENCHMARK "Build benchmark" OFF)
 
 
 find_package(Eigen3 3.3.9 REQUIRED NO_MODULE)
@@ -76,4 +77,12 @@ if(BUILD_TESTS)
     target_link_libraries(${test} PRIVATE ruckig Catch2::Catch2)
     add_test(NAME ${test} COMMAND ${test})
   endforeach()
+
+  if(BUILD_BENCHMARK)
+    add_executable(otg-benchmark "test/otg-benchmark.cpp")
+    if(Reflexxes)
+      target_compile_definitions(otg-benchmark PUBLIC WITH_REFLEXXES)
+    endif()
+    target_link_libraries(otg-benchmark PRIVATE ruckig Catch2::Catch2)
+  endif()
 endif()

README.md

@@ -152,7 +152,7 @@ Moreover, a range of additional parameter about the duration of the trajectory a
 
 ## Tests and Numerical Stability
 
-The current test suite validates over 1.008.000 (random) trajectories. The numerical exactness is tested for the position, velocity, acceleration, and time target to be within `1e-8`, for the velocity and acceleration limit to be withing `1e-9`, and for the jerk limit to be within a numerical error of `1e-12`. Ruckig presumes that the input values are within 5 orders of magnitude and that `jMax > 5e-2`. Note that Ruckig will also output values outside of this range, there is however no guarantee for correctness.
+The current test suite validates over 1.008.000 (random) trajectories. The numerical exactness is tested for the final position and final velocity to be within `1e-8`, for the velocity and acceleration limit to be withing `1e-9`, and for the final acceleration as well as jerk limit to be within a numerical error of `1e-12`. Ruckig presumes that the input values are within 5 orders of magnitude and that `jMax > 5e-2`. Note that Ruckig will also output values outside of this range, there is however no guarantee for correctness. The maximal supported trajectory duration is `1e12`.
 
 
 ## Development

include/ruckig/profile.hpp

@@ -25,7 +25,7 @@ struct Profile {
     //! Allow up to two segments of braking before the "correct" profile starts
     std::array<double, 2> t_brakes, j_brakes, a_brakes, v_brakes, p_brakes;
 
-    template<Teeth teeth>
+    template<Teeth teeth, Limits limits>
     bool check(double pf, double vf, double af, double jf, double vMax, double aMax) {
         if constexpr (teeth == Teeth::UDDU) {
             j = {jf, 0, -jf, 0, -jf, 0, jf};
@@ -42,22 +42,34 @@ struct Profile {
             if (t[i+1] < 0) {
                 return false;
             }
-
             t_sum[i+1] = t_sum[i] + t[i+1];
         }
         for (size_t i = 0; i < 7; i += 1) {
             a[i+1] = a[i] + t[i] * j[i];
             v[i+1] = v[i] + t[i] * (a[i] + t[i] * j[i] / 2);
             p[i+1] = p[i] + t[i] * (v[i] + t[i] * (a[i] / 2 + t[i] * j[i] / 6));
+
+            if constexpr (limits == Limits::ACC0_ACC1_VEL || limits == Limits::ACC0_VEL || limits == Limits::ACC1_VEL || limits == Limits::VEL) {
+                if (i == 2) {
+                    a[i+1] = 0.0;
+                }
+            }
         }
 
+        if (t_sum[6] > 1e12) { // For numerical reasons, equal to around 32000 years in SI units...
+            return false;
+        }
+
+        this->teeth = teeth;
+        this->limits = limits;
+
         // Velocity limit can be broken in the beginning if both initial velocity and acceleration are too high
-        // std::cout << std::setprecision(15) << "target: " << std::abs(p[7]-pf) << " " << std::abs(v[7] - vf) << " " << std::abs(a[7] - af) << std::endl;
+        // std::cout << std::setprecision(15) << "target: " << std::abs(p[7]-pf) << " " << std::abs(v[7] - vf) << " " << std::abs(a[7] - af) << " T: " << t_sum[6] << std::endl;
         const double vMaxAbs = std::abs(vMax) + 1e-9;
         const double aMaxAbs = std::abs(aMax) + 1e-9;
         return std::abs(p[7] - pf) < 1e-8
             && std::abs(v[7] - vf) < 1e-8
-            && std::abs(a[7] - af) < 1e-8
+            && std::abs(a[7] - af) < 1e-12 // This is not really needed, but we want to double check
             && std::abs(v[3]) < vMaxAbs
             && std::abs(v[4]) < vMaxAbs
             && std::abs(v[5]) < vMaxAbs
@@ -67,15 +79,16 @@ struct Profile {
             && std::abs(a[5]) < aMaxAbs;
     }
 
-    template<Teeth teeth>
+    template<Teeth teeth, Limits limits>
     inline bool check(double tf, double pf, double vf, double af, double jf, double vMax, double aMax) {
         // std::cout << std::setprecision(15) << "target: " << std::abs(t_sum[6]-tf) << " " << std::abs(p[7]-pf) << " " << std::abs(v[7] - vf) << " " << std::abs(a[7] - af) << std::endl;
-        return check<teeth>(pf, vf, af, jf, vMax, aMax) && (std::abs(t_sum[6] - tf) < 1e-8);
+        // Time doesn't need to be checked as every profile has one tf - ...
+        return check<teeth, limits>(pf, vf, af, jf, vMax, aMax); // && (std::abs(t_sum[6] - tf) < 1e-8);
     }
 
-    template<Teeth teeth>
+    template<Teeth teeth, Limits limits>
     inline bool check(double tf, double pf, double vf, double af, double jf, double vMax, double aMax, double jMax) {
-        return (std::abs(jf) < std::abs(jMax) + 1e-12) && check<teeth>(tf, pf, vf, af, jf, vMax, aMax);
+        return (std::abs(jf) < std::abs(jMax) + 1e-12) && check<teeth, limits>(tf, pf, vf, af, jf, vMax, aMax);
     }
 
     //! Integrate with constant jerk for duration t. Returns new position, new velocity, and new acceleration.

include/ruckig/roots.hpp

@@ -280,14 +280,15 @@ inline std::array<double, N-1> polyDeri(const std::array<double, N>& coeffs) {
     return deriv;
 }
 
-// template<size_t N>
-// inline std::array<double, N-1> polyMonicDeri(const std::array<double, N>& monic_coeffs) {
-//     std::array<double, N-1> deriv;
-//     for (int i = 0; i < N - 1; i++) {
-//         deriv[i] = (N - 1 - i) * monic_coeffs[i] / (N - 1);
-//     }
-//     return deriv;
-// }
+template<size_t N>
+inline std::array<double, N-1> polyMonicDeri(const std::array<double, N>& monic_coeffs) {
+    std::array<double, N-1> deriv;
+    deriv[0] = 1.0;
+    for (int i = 1; i < N - 1; i++) {
+        deriv[i] = (N - 1 - i) * monic_coeffs[i] / (N - 1);
+    }
+    return deriv;
+}
 
 // Safe Newton Method
 // Requirements: f(l)*f(h) <= 0

include/ruckig/ruckig.hpp

@@ -89,10 +89,10 @@ class Ruckig {
                     profiles[limiting_dof] = blocks[limiting_dof].p_min;
                 } break;
                 case 1: {
-                    profiles[limiting_dof] = blocks[limiting_dof].p_a.value();
+                    profiles[limiting_dof] = blocks[limiting_dof].a->profile;
                 } break;
                 case 2: {
-                    profiles[limiting_dof] = blocks[limiting_dof].p_b.value();
+                    profiles[limiting_dof] = blocks[limiting_dof].b->profile;
                 } break;
             }
             return true;

include/ruckig/steps.hpp

@@ -12,14 +12,19 @@ namespace ruckig {
 struct Block {
     struct Interval {
         double left, right; // [s]
+        Profile profile; // Profile corresponding to right (end) time
+
+        explicit Interval(double left, double right, const Profile& profile): left(left), right(right), profile(profile) { };
     };
 
     double t_min; // [s]
     Profile p_min; // Save min profile so that it doesn't need to be recalculated in Step2
 
-    // Max. 2 intervals can be blocked: a and b with corresponding profiles
+    // Max. 2 intervals can be blocked: a and b with corresponding profiles, the order does not matter
     std::optional<Interval> a, b;
-    std::optional<Profile> p_a, p_b;
+
+    explicit Block() { }
+    explicit Block(const Profile& p_min): p_min(p_min), t_min(p_min.t_sum[6] + p_min.t_brake.value_or(0.0)) { }
 
     bool is_blocked(double t) const {
         return (t < t_min) || (a && a->left < t && t < a->right) || (b && b->left < t && t < b->right);
@@ -59,8 +64,6 @@ class Step1 {
     std::array<Profile, 6> valid_profiles;
     size_t valid_profile_counter;
 
-    void add_profile(Profile profile, Limits limits, double jMax);
-
     void time_up_acc0_acc1_vel(Profile& profile, double vMax, double aMax, double jMax);
     void time_up_acc1_vel(Profile& profile, double vMax, double aMax, double jMax);
     void time_up_acc0_vel(Profile& profile, double vMax, double aMax, double jMax);
@@ -79,42 +82,23 @@ class Step1 {
     void time_down_acc0(Profile& profile, double vMin, double aMax, double jMax);
     void time_down_none(Profile& profile, double vMin, double aMax, double jMax);
 
-    template<size_t N, size_t left, size_t right, bool same_direction = true>
-    inline void add_block(double t_brake) {
-        if constexpr (N == 0) {
-            if (block.a) {
-                return;
-            }
-        } else {
-            if (block.b) {
-                return;
-            }
-        }
+    void add_profile(Profile profile, double jMax) {
+        profile.direction = (jMax > 0) ? Profile::Direction::UP : Profile::Direction::DOWN;
+        valid_profiles[valid_profile_counter] = profile;
+        ++valid_profile_counter;
+    }
 
-        double left_duration = valid_profiles[left].t_sum[6] + t_brake;
-        double right_duraction = valid_profiles[right].t_sum[6] + t_brake;
-        if constexpr (same_direction) {
-            if (valid_profiles[left].direction != valid_profiles[right].direction) {
-                return;
-            }
+    inline void add_interval(std::optional<Block::Interval>& interval, size_t left, size_t right, double t_brake) const {
+        if (interval) {
+            return;
         }
-
+
+        const double left_duration = valid_profiles[left].t_sum[6] + t_brake;
+        const double right_duraction = valid_profiles[right].t_sum[6] + t_brake;
         if (left_duration < right_duraction) {
-            if constexpr (N == 0) {
-                block.a = Block::Interval {left_duration, right_duraction};
-                block.p_a = valid_profiles[right];
-            } else {
-                block.b = Block::Interval {left_duration, right_duraction};
-                block.p_b = valid_profiles[right];
-            }
+            interval = Block::Interval(left_duration, right_duraction, valid_profiles[right]);
         } else {
-            if constexpr (N == 0) {
-                block.a = Block::Interval {right_duraction, left_duration};
-                block.p_a = valid_profiles[left];
-            } else {
-                block.b = Block::Interval {right_duraction, left_duration};
-                block.p_b = valid_profiles[left];
-            }
+            interval = Block::Interval(right_duraction, left_duration, valid_profiles[left]);
         }
     }
 
@@ -130,6 +114,7 @@ class Step1 {
 
 
 class Step2 {
+    using Limits = Profile::Limits;
     using Teeth = Profile::Teeth;
 
     double tf;

notebooks/ruckig-step1.nb

@@ -10,10 +10,10 @@
 NotebookFileLineBreakTest
 NotebookFileLineBreakTest
 NotebookDataPosition[       158,          7]
-NotebookDataLength[    127600,       3113]
-NotebookOptionsPosition[    125362,       3071]
-NotebookOutlinePosition[    125756,       3087]
-CellTagsIndexPosition[    125713,       3084]
+NotebookDataLength[    127760,       3114]
+NotebookOptionsPosition[    125522,       3072]
+NotebookOutlinePosition[    125916,       3088]
+CellTagsIndexPosition[    125873,       3085]
 WindowFrame->Normal*)
 
 (* Beginning of Notebook Content *)
@@ -422,20 +422,21 @@ Cell[BoxData[
    3.817629903651003*^9, 3.81762990789653*^9}, 3.817630008980739*^9, {
    3.8176308336239557`*^9, 3.8176308355038147`*^9}, 3.8176412855214357`*^9, {
    3.819480271236746*^9, 3.819480275524906*^9}},
- CellLabel->"In[34]:=",ExpressionUUID->"ccba75f7-892e-4119-93ec-41811b2e330e"],
+ CellLabel->"In[26]:=",ExpressionUUID->"ccba75f7-892e-4119-93ec-41811b2e330e"],
 
 Cell["\<\
 Information Roots are max. 4th Order
-- (1) Case 1: Acc0_Acc1_Vel: Solution 1
-- (2) Case 2: Acc0_Acc1: Solution 1/2 (UDDU)
-- (3) Case 3: Acc1_Vel: Solution 2
-- (4) Case 4: Acc0_Vel: Solution 1/2
-- (5) Case 5: Vel: Solution 3/4
-- (6) Case 6a: Acc1: Root in t1
-- (7) Case 6b: Acc1: Root in t1 (UDUD for af != 0)
-- (8) Case 7: Acc0: Root in t5
-- (9) Case 8: None: Root in t1, (UDDU), (UDUD for af != 0)
-- (10) Case 8: None: Root in t1, (UDDU), (UDUD for af != 0) with a0==af==0\
+- (1) Case 1: Acc0_Acc1_Vel: Solution 1 (Length 1)
+- (2) Case 2: Acc0_Acc1: Solution 1/2 (UDDU) (Length 2)
+- (3) Case 3: Acc1_Vel: Solution 2 (Length 2)
+- (4) Case 4: Acc0_Vel: Solution 1/2 (Length 2)
+- (5) Case 5: Vel: Solution 3/4 (Length 4)
+- (6) Case 6a: Acc1: Root in t1 (Length 4)
+- (7) Case 6b: Acc1: Root in t1 (UDUD for af != 0) (Length 4)
+- (8) Case 7: Acc0: Root in t5 (Length 4)
+- (9) Case 8: None: Root in t1, (UDDU), (UDUD for af != 0) (Length 4)
+- (10) Case 8: None: Root in t1, (UDDU), (UDUD for af != 0) with a0==af==0 \
+(Length 4)\
 \>", "Text",
  CellChangeTimes->{{3.8174632785384893`*^9, 3.817463379445586*^9}, {
    3.8174635726156178`*^9, 3.817463721259947*^9}, {3.8174665706088743`*^9, 
@@ -447,8 +448,8 @@ Information Roots are max. 4th Order
    3.817630011713861*^9, 3.8176300169571753`*^9}, {3.817630830414349*^9, 
    3.81763085334088*^9}, {3.8176412891511297`*^9, 3.8176412957587967`*^9}, 
    3.817700725859343*^9, {3.817700767970825*^9, 3.81770076813795*^9}, {
-   3.819480286564745*^9, 
-   3.819480293963434*^9}},ExpressionUUID->"e82cb2e9-b4f4-4bc2-9a74-\
+   3.819480286564745*^9, 3.819480293963434*^9}, {3.819707143963235*^9, 
+   3.8197071909725227`*^9}},ExpressionUUID->"e82cb2e9-b4f4-4bc2-9a74-\
 547157d3c4dc"],
 
 Cell["\<\
@@ -3070,7 +3071,7 @@ Cell[BoxData[
 }, Open  ]]
 },
 WindowSize->{872, 747},
-WindowMargins->{{42, Automatic}, {Automatic, 221}},
+WindowMargins->{{81, Automatic}, {Automatic, 337}},
 FrontEndVersion->"12.1 for Mac OS X x86 (64-bit) (June 19, 2020)",
 StyleDefinitions->"Default.nb",
 ExpressionUUID->"c8b287f8-8b4a-4e98-ab1e-df7dcd999be5"
@@ -3089,31 +3090,31 @@ Notebook[{
 Cell[558, 20, 250, 8, 81, "Text",ExpressionUUID->"f0efc888-777d-4c66-b8e6-2db88daacc31"],
 Cell[811, 30, 7240, 225, 875, "Input",ExpressionUUID->"d346b209-9dd3-47cf-8f1e-f04bc3bf980a"],
 Cell[8054, 257, 6852, 167, 367, "Input",ExpressionUUID->"ccba75f7-892e-4119-93ec-41811b2e330e"],
-Cell[14909, 426, 1346, 25, 265, "Text",ExpressionUUID->"e82cb2e9-b4f4-4bc2-9a74-547157d3c4dc"],
-Cell[16258, 453, 249, 7, 58, "Text",ExpressionUUID->"2bb13fa1-f9ef-4a5d-ac3b-101e020ad421"],
+Cell[14909, 426, 1506, 26, 265, "Text",ExpressionUUID->"e82cb2e9-b4f4-4bc2-9a74-547157d3c4dc"],
+Cell[16418, 454, 249, 7, 58, "Text",ExpressionUUID->"2bb13fa1-f9ef-4a5d-ac3b-101e020ad421"],
 Cell[CellGroupData[{
-Cell[16532, 464, 1796, 38, 115, "Input",ExpressionUUID->"f20278fb-5b15-4c9c-bd38-9f1d3931e2f3"],
-Cell[18331, 504, 942, 13, 34, "Output",ExpressionUUID->"d3d06386-dc01-4a4e-ab6e-96f532c5d53c"],
-Cell[19276, 519, 973, 15, 54, "Output",ExpressionUUID->"44ab03ef-9010-4438-9728-59e5a07bb808"]
+Cell[16692, 465, 1796, 38, 115, "Input",ExpressionUUID->"f20278fb-5b15-4c9c-bd38-9f1d3931e2f3"],
+Cell[18491, 505, 942, 13, 34, "Output",ExpressionUUID->"d3d06386-dc01-4a4e-ab6e-96f532c5d53c"],
+Cell[19436, 520, 973, 15, 54, "Output",ExpressionUUID->"44ab03ef-9010-4438-9728-59e5a07bb808"]
 }, Open  ]],
-Cell[20264, 537, 166, 3, 35, "Text",ExpressionUUID->"8d7af2fe-a47e-4ef4-b28a-db0dc764dd0f"],
+Cell[20424, 538, 166, 3, 35, "Text",ExpressionUUID->"8d7af2fe-a47e-4ef4-b28a-db0dc764dd0f"],
 Cell[CellGroupData[{
-Cell[20455, 544, 2080, 49, 159, "Input",ExpressionUUID->"ca8158ca-2cc6-4c2b-92b0-ae9728cb14cd"],
-Cell[22538, 595, 2388, 60, 102, "Output",ExpressionUUID->"f542bf2e-909f-4382-b877-d01cf719c719"]
+Cell[20615, 545, 2080, 49, 159, "Input",ExpressionUUID->"ca8158ca-2cc6-4c2b-92b0-ae9728cb14cd"],
+Cell[22698, 596, 2388, 60, 102, "Output",ExpressionUUID->"f542bf2e-909f-4382-b877-d01cf719c719"]
 }, Open  ]],
 Cell[CellGroupData[{
-Cell[24963, 660, 1282, 26, 94, "Input",ExpressionUUID->"02ddacce-ea40-40b5-bbc9-e4e0e8a8500b"],
-Cell[26248, 688, 1304, 29, 58, "Output",ExpressionUUID->"77115233-e544-4ee6-a2f9-7f5622bcfff6"]
+Cell[25123, 661, 1282, 26, 94, "Input",ExpressionUUID->"02ddacce-ea40-40b5-bbc9-e4e0e8a8500b"],
+Cell[26408, 689, 1304, 29, 58, "Output",ExpressionUUID->"77115233-e544-4ee6-a2f9-7f5622bcfff6"]
 }, Open  ]],
 Cell[CellGroupData[{
-Cell[27589, 722, 1671, 37, 94, "Input",ExpressionUUID->"bbf1a9de-0221-4992-b9c6-d1f769287d62"],
-Cell[29263, 761, 2462, 53, 98, "Output",ExpressionUUID->"105051c1-dd97-49a1-a1ea-1856a027794e"],
-Cell[31728, 816, 829, 13, 34, "Output",ExpressionUUID->"ee561e7d-9711-4560-80d9-d30648c0977e"]
+Cell[27749, 723, 1671, 37, 94, "Input",ExpressionUUID->"bbf1a9de-0221-4992-b9c6-d1f769287d62"],
+Cell[29423, 762, 2462, 53, 98, "Output",ExpressionUUID->"105051c1-dd97-49a1-a1ea-1856a027794e"],
+Cell[31888, 817, 829, 13, 34, "Output",ExpressionUUID->"ee561e7d-9711-4560-80d9-d30648c0977e"]
 }, Open  ]],
-Cell[32572, 832, 154, 3, 30, "Input",ExpressionUUID->"f0b1e6e6-f499-4d6a-bda7-66c7bc16539e"],
+Cell[32732, 833, 154, 3, 30, "Input",ExpressionUUID->"f0b1e6e6-f499-4d6a-bda7-66c7bc16539e"],
 Cell[CellGroupData[{
-Cell[32751, 839, 152, 3, 30, "Input",ExpressionUUID->"7ceb179a-1f49-4fe0-8dd4-64829d564d38"],
-Cell[32906, 844, 92440, 2224, 2982, "Output",ExpressionUUID->"463aab8c-0e56-4519-9577-c051f8a24fdf"]
+Cell[32911, 840, 152, 3, 30, "Input",ExpressionUUID->"7ceb179a-1f49-4fe0-8dd4-64829d564d38"],
+Cell[33066, 845, 92440, 2224, 2982, "Output",ExpressionUUID->"463aab8c-0e56-4519-9577-c051f8a24fdf"]
 }, Open  ]]
 }
 ]

src/brake.cpp

@@ -29,24 +29,7 @@ void Brake::acceleration_brake(double v0, double a0, double vMax, double aMax, d
         t_brake[1] = std::min(t_to_v_max_while_a_max, t_to_v_max_in_reverse_j_direction) - eps;
 
     } else if ((v_at_a_max > vMax && jMax > 0) || (v_at_a_max < vMax && jMax < 0)) {
-        double t_to_other_a_max = (a0 + aMax) / jMax - eps;
-        double t_to_v_max = a0/jMax + std::sqrt(a0*a0 + 2 * jMax * (v0 - vMax)) / std::abs(jMax);
-        double t_to_v_max_brake_for_other = a0/jMax + std::sqrt(a0*a0/2 + jMax * (v0 + vMax)) / std::abs(jMax);
-
-        if (t_to_v_max_brake_for_other < t_to_other_a_max && t_to_v_max_brake_for_other < t_to_v_max) {
-            t_brake[0] = t_to_v_max_brake_for_other - eps;
-
-        } else if (t_to_other_a_max < t_to_v_max) {
-            double v_at_a_other_a_max = v_at_t(v0, a0, -jMax, t_to_other_a_max);
-            double t_to_v_max_while_a_max = (v_at_a_other_a_max - vMax)/aMax;
-            double t_to_v_max_brake_for_other_a_max = -aMax/(2*jMax) + (vMax + v_at_a_other_a_max)/aMax;
-
-            t_brake[0] = t_to_other_a_max - eps;
-            t_brake[1] = std::min(t_to_v_max_while_a_max, t_to_v_max_brake_for_other_a_max);
-
-        } else {
-            t_brake[0] = t_to_v_max + eps;
-        }
+        velocity_brake(v0, a0, vMax, aMax, jMax, t_brake, j_brake);
     }
 }
 
@@ -61,13 +44,11 @@ void Brake::velocity_brake(double v0, double a0, double vMax, double aMax, doubl
         double t_to_v_max_while_a_max = (v_at_a_max - vMax)/aMax;
         double t_to_v_max_in_reverse_j_direction = -aMax/(2*jMax) + (v_at_a_max + vMax)/aMax;
 
-        t_brake[0] = t_to_a_max;
+        t_brake[0] = t_to_a_max - eps;
         t_brake[1] = std::min(t_to_v_max_while_a_max, t_to_v_max_in_reverse_j_direction);
     } else {
-        t_brake[0] = std::min(t_to_v_max_in_j_direction, t_to_v_max_in_reverse_j_direction);
+        t_brake[0] = std::min(t_to_v_max_in_j_direction, t_to_v_max_in_reverse_j_direction) - eps;
     }
-
-    t_brake[0] = std::max(t_brake[0] - eps, 0.0);
 }
 
 void Brake::get_brake_trajectory(double v0, double a0, double vMax, double vMin, double aMax, double jMax, std::array<double, 2>& t_brake, std::array<double, 2>& j_brake) {
@@ -87,7 +68,7 @@ void Brake::get_brake_trajectory(double v0, double a0, double vMax, double vMin,
     } else if ((v0 > vMax && (a0 > 0 || a0_sq_jMax < v0)) || (a0 > 0 && a0_sq_jMax > -v0)) {
         velocity_brake(v0, a0, vMax, aMax, jMax, t_brake, j_brake);
 
-    } else if ((v0 < vMin && (a0 < 0 || a0_sq_jMax > v0)) || (a0 < 0 && a0_sq_jMax > v0)) {
+    } else if ((v0 < vMin && (a0 < 0 || a0_sq_jMax < -v0)) || (a0 < 0 && a0_sq_jMax > v0)) {
         velocity_brake(v0, a0, vMin, -aMax, -jMax, t_brake, j_brake);
     }
 }