fix some edge cases

include/ruckig/steps.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <array>
+#include <limits>
 #include <optional>
 
 #include <ruckig/trajectory.hpp>

include/ruckig/trajectory.hpp

@@ -52,6 +52,24 @@ struct Profile {
             t_sum[i+1] = t_sum[i] + t[i+1];
         }
 
+        if constexpr (limits == Limits::ACC0_ACC1_VEL || limits == Limits::ACC0_VEL || limits == Limits::ACC1_VEL || limits == Limits::VEL) {
+            if (t[3] < std::numeric_limits<double>::epsilon()) {
+                return false;
+            }
+        }
+
+        if constexpr (limits == Limits::ACC0 || limits == Limits::ACC0_ACC1) {
+            if (t[1] < std::numeric_limits<double>::epsilon()) {
+                return false;
+            }
+        }
+
+        if constexpr (limits == Limits::ACC1 || limits == Limits::ACC0_ACC1) {
+            if (t[5] < std::numeric_limits<double>::epsilon()) {
+                return false;
+            }
+        }
+
         if (t_sum[6] > 1e12) { // For numerical reasons, is that needed?
             return false;
         }

src/step1.cpp

@@ -167,6 +167,11 @@ void Step1::time_up_acc1(Profile& profile, double vMax, double aMax, double jMax
             t -= orig / deriv;
         }
 
+        // Corresponds to inverse ACC0
+        if (t < DBL_EPSILON && (-af_af + 2*jMax*(vf - v0) + aMax_aMax) < DBL_EPSILON) {
+            continue;
+        }
+
         profile.t[0] = t;
         profile.t[1] = 0;
         profile.t[2] = (a0 + aMax)/jMax + t;
@@ -228,6 +233,42 @@ void Step1::time_up_none(Profile& profile, double vMax, double aMax, double jMax
             }
             return;
         }
+
+        if (std::abs(a0 - af) < DBL_EPSILON && std::abs(v0 + vf) < DBL_EPSILON && std::abs(p0 - pf) < DBL_EPSILON) {
+            const double h1 = std::sqrt(a0_a0 - 2*jMax*v0);
+
+            // Solution 3
+            {
+                profile.t[0] = -(a0 + h1)/jMax;
+                profile.t[1] = 0;
+                profile.t[2] = profile.t[0];
+                profile.t[3] = 0;
+                profile.t[4] = 0;
+                profile.t[5] = 0;
+                profile.t[6] = 0;
+
+                if (profile.check<JerkSigns::UDDU, Limits::NONE>(pf, vf, af, jMax, vMax, aMax)) {
+                    add_profile(profile, jMax);
+                }
+            }
+
+            // Solution 4
+            {
+                profile.t[0] = -(a0 - h1)/jMax;
+                profile.t[1] = 0;
+                profile.t[2] = profile.t[0];
+                profile.t[3] = 0;
+                profile.t[4] = 0;
+                profile.t[5] = 0;
+                profile.t[6] = 0;
+
+                if (profile.check<JerkSigns::UDDU, Limits::NONE>(pf, vf, af, jMax, vMax, aMax)) {
+                    add_profile(profile, jMax);
+                }
+            }
+
+            return;
+        }
     }
 
     // UDDU / UDUD modern

test/otg-test.cpp

@@ -299,6 +299,39 @@ TEST_CASE("comparison_3" * doctest::description("Comparison with Reflexxes with
 }
 #endif
 
+TEST_CASE("random_discrete_3" * doctest::description("Random discrete input with 3 DoF and target velocity, acceleration")) {
+    constexpr size_t DOFs {3};
+    Ruckig<DOFs, true> otg {0.005};
+    InputParameter<DOFs> input;
+
+    std::uniform_int_distribution<int> position_discrete_dist(-1, 1);
+    std::uniform_int_distribution<int> dynamic_discrete_dist(-1, 1);
+    std::uniform_int_distribution<int> limit_discrete_dist(1, 2);
+
+    Randomizer<DOFs, decltype(position_discrete_dist)> p { position_discrete_dist, seed };
+    Randomizer<DOFs, decltype(dynamic_discrete_dist)> d { dynamic_discrete_dist, seed + 1 };
+    Randomizer<DOFs, decltype(limit_discrete_dist)> l { limit_discrete_dist, seed + 2 };
+
+    for (size_t i = 0; i < 0; ++i) {
+        p.fill(input.current_position);
+        d.fill(input.current_velocity);
+        d.fill(input.current_acceleration);
+        p.fill(input.target_position);
+        d.fill(input.target_velocity);
+        d.fill(input.target_acceleration);
+        l.fill(input.max_velocity, input.target_velocity);
+        l.fill(input.max_acceleration, input.target_acceleration);
+        l.fill(input.max_jerk);
+
+        if (!otg.validate_input(input)) {
+            --i;
+            continue;
+        }
+
+        check_calculation(otg, input);
+    }
+}
+
 
 int main(int argc, char** argv) {
     doctest::Context context;

test/otg_plot.py

@@ -26,7 +26,7 @@ def walk_through_trajectory(otg, inp, print_table=True):
         inp.current_position = out.new_position
         inp.current_velocity = out.new_velocity
         inp.current_acceleration = out.new_acceleration
-        
+
         if print_table:
             jerk = (old_acc - out.new_acceleration[print_dof]) / otg.delta_time
             old_acc = out.new_acceleration[print_dof]
@@ -106,15 +106,15 @@ def print_input_for_mathematica(inp, dof, tf=None):
 
 if __name__ == '__main__':
     inp = InputParameter()
-    inp.current_position = [3.2654259134071, 0.149631978054923, -1.85294403350041]
-    inp.current_velocity = [-0.631048955228958, -1.02935440881227, -0.485986175478385]
-    inp.current_acceleration = [-0.00247258010611476, 0, -1.09040067528293]
-    inp.target_position = [1.32072926391243, -1.50720441044648, 10.2222552440055]
-    inp.target_velocity = [-1.13931998000192, 1.1817053469200048, 1.57160490634656]
-    inp.target_acceleration = [0.0, 0, 0.0]
-    inp.max_velocity = [12.5582057488951, 3.98686538344635, 8.77760182258326]
-    inp.max_acceleration = [0.48202376602604, 11.3641030476307, 11.4692498916033]
-    inp.max_jerk = [6.80578669166441, 0.7045088159413865, 8.62520213101531]
+    inp.current_position = [0, 1, 1]
+    inp.current_velocity = [1, -1, -1]
+    inp.current_acceleration = [0, -1, 0]
+    inp.target_position = [1, -1, 0]
+    inp.target_velocity = [-1, 0, 0]
+    inp.target_acceleration = [-1, -1, 0]
+    inp.max_velocity = [3, 1, 1]
+    inp.max_acceleration = [2, 3, 1]
+    inp.max_jerk = [1, 1, 1]
 
     print_input_for_mathematica(inp, 1)
 
@@ -128,6 +128,6 @@ def print_input_for_mathematica(inp, dof, tf=None):
     # print(otg.get_position_range())
 
     print(f'Calculation duration: {out_list[0].calculation_duration:0.1f} [µs]')
-    print(f'Trajectory duration: {out_list[0].duration:0.4f} [s]')
+    print(f'Trajectory duration: {out_list[0].trajectory.duration:0.4f} [s]')
 
     plot_trajectory(t_list, out_list)