Increase stability

pantor · Jan 13, 2021 · 177c2d8 · 177c2d8
1 parent 0eb0e12
commit 177c2d8
Show file tree

Hide file tree

Showing 8 changed files with 2,360 additions and 154 deletions.
diff --git a/include/ruckig/profile.hpp b/include/ruckig/profile.hpp
@@ -51,11 +51,20 @@ struct Profile {
             p[i+1] = p[i] + t[i] * (v[i] + t[i] * (a[i] / 2 + t[i] * j[i] / 6));
         }
 
-        // Velocity and acceleration limits can be broken in the beginning if the initial velocity and acceleration are too high
+        // 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;
-        return std::all_of(v.begin() + 3, v.end(), [vMax](double vm){ return std::abs(vm) < std::abs(vMax) + 1e-9; })
-            && std::all_of(a.begin() + 2, a.end(), [aMax](double am){ return std::abs(am) < std::abs(aMax) + 1e-9; })
-            && std::abs(p[7] - pf) < 1e-8 && std::abs(v[7] - vf) < 1e-8 && std::abs(a[7] - af) < 1e-8;
+        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(v[3]) < vMaxAbs
+            && std::abs(v[4]) < vMaxAbs
+            && std::abs(v[5]) < vMaxAbs
+            && std::abs(v[6]) < vMaxAbs
+            && std::abs(a[1]) < aMaxAbs
+            && std::abs(a[3]) < aMaxAbs
+            && std::abs(a[5]) < aMaxAbs;
     }
 
     template<Teeth teeth>

diff --git a/include/ruckig/roots.hpp b/include/ruckig/roots.hpp
@@ -195,11 +195,11 @@ inline std::set<double> solveQuartMonic(double a, double b, double c, double d)
     // h1 + h2 = y && h1*h2 = d  <=>  h^2 - y*h + d = 0    (h === q)
 
     D = y * y - 4.0 * d;
-    if (std::abs(D) < DBL_EPSILON) { //In other words: D == 0
+    if (std::abs(D) < DBL_EPSILON) {
         q1 = q2 = y * 0.5;
         // g1 + g2 = a && g1 + g2 = b - y   <=>   g^2 - a*g + b - y = 0    (p === g)
         D = a * a - 4.0 * (b - y);
-        if (std::abs(D) < DBL_EPSILON) { //In other words: D == 0
+        if (std::abs(D) < DBL_EPSILON) {
             p1 = p2 = a * 0.5;
         } else {
             sqrtD = std::sqrt(D);
@@ -215,9 +215,11 @@ inline std::set<double> solveQuartMonic(double a, double b, double c, double d)
         p2 = (c - a * q2) / (q1 - q2);
     }
 
+    constexpr double eps {2 * DBL_EPSILON};
+
     // Solve the quadratic equation: x^2 + p1*x + q1 = 0
     D = p1 * p1 - 4.0 * q1;
-    if (std::abs(D) < DBL_EPSILON) {
+    if (std::abs(D) < eps) {
         roots.insert(-p1 * 0.5);
     } else if (D > 0.0) {
         sqrtD = std::sqrt(D);
@@ -227,7 +229,7 @@ inline std::set<double> solveQuartMonic(double a, double b, double c, double d)
 
     // Solve the quadratic equation: x^2 + p2*x + q2 = 0
     D = p2 * p2 - 4.0 * q2;
-    if (std::abs(D) < DBL_EPSILON) {
+    if (std::abs(D) < eps) {
         roots.insert(-p2 * 0.5);
     } else if (D > 0.0) {
         sqrtD = std::sqrt(D);

diff --git a/include/ruckig/steps.hpp b/include/ruckig/steps.hpp
@@ -29,7 +29,7 @@ struct Block {
 
 //! Calculates (pre-) trajectory to get current state below the limits
 class Brake {
-    static constexpr double eps {3e-15};
+    static constexpr double eps {2e-14};
 
     static void acceleration_brake(double v0, double a0, double vMax, double aMax, double jMax, std::array<double, 2>& t_brake, std::array<double, 2>& j_brake);
     static void velocity_brake(double v0, double a0, double vMax, double aMax, double jMax, std::array<double, 2>& t_brake, std::array<double, 2>& j_brake);