robustify step2 vel uddu

pantor · Sep 13, 2022 · 0347d7d · 0347d7d
1 parent d7838b0
commit 0347d7d
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 1 deletion.
diff --git a/src/ruckig/position-step2.cpp b/src/ruckig/position-step2.cpp
@@ -350,7 +350,9 @@ bool PositionStep2::time_vel(Profile& profile, double vMax, double vMin, double
                 const double h1 = std::sqrt((a0_a0 + af_af)/(2*jMax_jMax) + (2*a0*t + jMax*t*t - vd)/jMax);
                 const double orig = -pd - (2*a0_p3 + 4*af_p3 + 24*a0*jMax*t*(af + jMax*(h1 + t - tf)) + 6*a0_a0*(af + jMax*(2*t - tf)) + 6*(a0_a0 + af_af)*jMax*h1 + 12*af*jMax*(jMax*t*t - vd) + 12*jMax_jMax*(jMax*t*t*(h1 + t - tf) - tf*v0 - h1*vd))/(12*jMax_jMax);
                 const double deriv_newton = -(a0 + jMax*t)*(3*(h1 + t) - 2*tf + (a0 + 2*af)/jMax);
-                t -= orig / deriv_newton;
+                if (!std::isnan(orig) && !std::isnan(deriv_newton) && std::abs(deriv_newton) > DBL_EPSILON) {
+                    t -= orig / deriv_newton;
+                }
             }
 
             if (t > tf || std::isnan(t)) {

diff --git a/test/test-target-known.cpp b/test/test-target-known.cpp
@@ -341,6 +341,27 @@ TEST_CASE("known" * doctest::description("Known examples")) {
     input.duration_discretization = ruckig::DurationDiscretization::Discrete;
     check_duration(otg, input, 0.048);
 
+    input.current_position = {-19.93333333333424, -0.4983333333333563, 0};
+    input.current_velocity = {10, 0.25, 0};
+    input.current_acceleration = {0, 0, 0};
+    input.target_position = {20, 0.5, 0.2};
+    input.target_velocity = {10, 0.25, 0};
+    input.target_acceleration = {0, 0, 0};
+    input.max_velocity = {10, 10, 10};
+    input.max_acceleration = {15, 15, 15};
+    input.max_jerk = {15, 5, 2};
+    input.control_interface = ControlInterface::Position;
+    input.duration_discretization = ruckig::DurationDiscretization::Continuous;
+
+    Trajectory<3> traj;
+    otg.calculate(input, traj);
+    check_duration(otg, input, 3.99333);
+
+    std::array<double, 3> new_position, new_velocity, new_acceleration;
+    traj.at_time(traj.get_duration() / 2, new_position, new_velocity, new_acceleration);
+    CHECK( new_position[0] == doctest::Approx(0.033333) );
+    CHECK( new_position[1] == doctest::Approx(0.000833) );
+
 
     RuckigThrow<38> otg38 {0.004};
     InputParameter<38> input38;