From ac8d379013dcc0f1ecaa95dbc5bfe2cbf845f638 Mon Sep 17 00:00:00 2001 From: pantor Date: Thu, 14 Jan 2021 11:50:19 +0100 Subject: [PATCH] clean root finding --- README.md | 2 +- include/ruckig/roots.hpp | 21 +- include/ruckig/ruckig.hpp | 1 - include/ruckig/steps.hpp | 10 + notebooks/newton-step.nb | 489 ++++++++++++++++++++++++++++++++++++++ notebooks/ruckig-step2.nb | 115 ++++----- src/step1.cpp | 47 ++-- src/step2.cpp | 123 +++++----- test/otg-test.cpp | 4 +- test/otg_plot.py | 28 +-- 10 files changed, 684 insertions(+), 156 deletions(-) create mode 100644 notebooks/newton-step.nb diff --git a/README.md b/README.md index 9afbd606..90a9d870 100644 --- a/README.md +++ b/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. 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 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. ## Development diff --git a/include/ruckig/roots.hpp b/include/ruckig/roots.hpp index 24e518c2..c7b5a1f7 100644 --- a/include/ruckig/roots.hpp +++ b/include/ruckig/roots.hpp @@ -274,13 +274,21 @@ inline double polyEval(std::array p, double x) { template inline std::array polyDeri(const std::array& coeffs) { std::array deriv; - int horder = N - 1; - for (int i = 0; i < horder; i++) { - deriv[i] = (horder - i) * coeffs[i]; + for (int i = 0; i < N - 1; i++) { + deriv[i] = (N - 1 - i) * coeffs[i]; } return deriv; } +// template +// inline std::array polyMonicDeri(const std::array& monic_coeffs) { +// std::array deriv; +// for (int i = 0; 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 template @@ -344,12 +352,15 @@ inline double safeNewton(const F& func, const DF& dfunc, const double &l, const // Calculate a single zero of polynom p(x) inside [lbound, ubound] // Requirements: p(lbound)*p(ubound) < 0, lbound < ubound + +constexpr double tolerance {1e-14}; + template -inline double shrinkInterval(const std::array& p, double lbound, double ubound, double tol = 1e-14) { +inline double shrinkInterval(const std::array& p, double lbound, double ubound) { auto deriv = polyDeri(p); auto func = [&p](double x) { return polyEval(p, x); }; auto dfunc = [&deriv](double x) { return polyEval(deriv, x); }; - return safeNewton(func, dfunc, lbound, ubound, tol, maxDblIts); + return safeNewton(func, dfunc, lbound, ubound, tolerance, maxDblIts); } } // namespace Roots diff --git a/include/ruckig/ruckig.hpp b/include/ruckig/ruckig.hpp index f8181521..3fbf7cae 100644 --- a/include/ruckig/ruckig.hpp +++ b/include/ruckig/ruckig.hpp @@ -84,7 +84,6 @@ class Ruckig { t_sync = possible_t_sync; limiting_dof = std::ceil((i + 1.0) / 3) - 1; - // std::cout << "sync: " << limiting_dof << " " << i % 3 << " " << t_sync << std::endl; switch (i % 3) { case 0: { profiles[limiting_dof] = blocks[limiting_dof].p_min; diff --git a/include/ruckig/steps.hpp b/include/ruckig/steps.hpp index 83d8cef3..2e94a9f0 100644 --- a/include/ruckig/steps.hpp +++ b/include/ruckig/steps.hpp @@ -81,6 +81,16 @@ class Step1 { template inline void add_block(double t_brake) { + if constexpr (N == 0) { + if (block.a) { + return; + } + } else { + if (block.b) { + return; + } + } + 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) { diff --git a/notebooks/newton-step.nb b/notebooks/newton-step.nb new file mode 100644 index 00000000..c220fb53 --- /dev/null +++ b/notebooks/newton-step.nb @@ -0,0 +1,489 @@ +(* Content-type: application/vnd.wolfram.mathematica *) + +(*** Wolfram Notebook File ***) +(* http://www.wolfram.com/nb *) + +(* CreatedBy='Mathematica 12.1' *) + +(*CacheID: 234*) +(* Internal cache information: +NotebookFileLineBreakTest +NotebookFileLineBreakTest +NotebookDataPosition[ 158, 7] +NotebookDataLength[ 16370, 481] +NotebookOptionsPosition[ 15603, 461] +NotebookOutlinePosition[ 15998, 477] +CellTagsIndexPosition[ 15955, 474] +WindowFrame->Normal*) + +(* Beginning of Notebook Content *) +Notebook[{ +Cell[BoxData[{ + RowBox[{ + RowBox[{"jerkProfileUDDU", "=", + RowBox[{"{", + RowBox[{"jMax", ",", "0", ",", + RowBox[{"-", "jMax"}], ",", "0", ",", + RowBox[{"-", "jMax"}], ",", "0", ",", "jMax"}], "}"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"jerkProfileUDUD", "=", + RowBox[{"{", + RowBox[{"jMax", ",", "0", ",", + RowBox[{"-", "jMax"}], ",", "0", ",", "jMax", ",", "0", ",", + RowBox[{"-", "jMax"}]}], "}"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"jerkProfile", "=", "jerkProfileUDUD"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"a1", "=", + RowBox[{"a0", "+", + RowBox[{"t1", " ", + RowBox[{"jerkProfile", "[", + RowBox[{"[", "1", "]"}], "]"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"a2", "=", + RowBox[{"a1", "+", + RowBox[{"t2", " ", + RowBox[{"jerkProfile", "[", + RowBox[{"[", "2", "]"}], "]"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"a3", "=", + RowBox[{"a2", "+", + RowBox[{"t3", " ", + RowBox[{"jerkProfile", "[", + RowBox[{"[", "3", "]"}], "]"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"a4", "=", + RowBox[{"a3", "+", + RowBox[{"t4", " ", + RowBox[{"jerkProfile", "[", + RowBox[{"[", "4", "]"}], "]"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"a5", "=", + RowBox[{"a4", "+", + RowBox[{"t5", " ", + RowBox[{"jerkProfile", "[", + RowBox[{"[", "5", "]"}], "]"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"a6", "=", + RowBox[{"a5", "+", + RowBox[{"t6", " ", + RowBox[{"jerkProfile", "[", + RowBox[{"[", "6", "]"}], "]"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"a7", "=", + RowBox[{"a6", "+", + RowBox[{"t7", " ", + RowBox[{"jerkProfile", "[", + RowBox[{"[", "7", "]"}], "]"}]}]}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"v1", "=", + RowBox[{"v0", "+", + RowBox[{"t1", " ", "a0"}], "+", + RowBox[{ + FractionBox["1", "2"], + SuperscriptBox["t1", "2"], + RowBox[{"jerkProfile", "[", + RowBox[{"[", "1", "]"}], "]"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"v2", "=", + RowBox[{"v1", "+", + RowBox[{"t2", " ", "a1"}], "+", + RowBox[{ + FractionBox["1", "2"], + SuperscriptBox["t2", "2"], + RowBox[{"jerkProfile", "[", + RowBox[{"[", "2", "]"}], "]"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"v3", "=", + RowBox[{"v2", "+", + RowBox[{"t3", " ", "a2"}], "+", + RowBox[{ + FractionBox["1", "2"], + SuperscriptBox["t3", "2"], + RowBox[{"jerkProfile", "[", + RowBox[{"[", "3", "]"}], "]"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"v4", "=", + RowBox[{"v3", "+", + RowBox[{"t4", " ", "a3"}], "+", + RowBox[{ + FractionBox["1", "2"], + SuperscriptBox["t4", "2"], + RowBox[{"jerkProfile", "[", + RowBox[{"[", "4", "]"}], "]"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"v5", "=", + RowBox[{"v4", "+", + RowBox[{"t5", " ", "a4"}], "+", + RowBox[{ + FractionBox["1", "2"], + SuperscriptBox["t5", "2"], + RowBox[{"jerkProfile", "[", + RowBox[{"[", "5", "]"}], "]"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"v6", "=", + RowBox[{"v5", "+", + RowBox[{"t6", " ", "a5"}], "+", + RowBox[{ + FractionBox["1", "2"], + SuperscriptBox["t6", "2"], + RowBox[{"jerkProfile", "[", + RowBox[{"[", "6", "]"}], "]"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"v7", "=", + RowBox[{"v6", "+", + RowBox[{"t7", " ", "a6"}], "+", + RowBox[{ + FractionBox["1", "2"], + SuperscriptBox["t7", "2"], + RowBox[{"jerkProfile", "[", + RowBox[{"[", "7", "]"}], "]"}]}]}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"p1", "=", + RowBox[{"p0", "+", + RowBox[{"t1", " ", "v0"}], "+", + RowBox[{ + FractionBox["1", "2"], + SuperscriptBox["t1", "2"], " ", "a0"}], "+", + RowBox[{ + FractionBox["1", "6"], + SuperscriptBox["t1", "3"], + RowBox[{"jerkProfile", "[", + RowBox[{"[", "1", "]"}], "]"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"p2", "=", + RowBox[{"p1", "+", + RowBox[{"t2", " ", "v1"}], "+", + RowBox[{ + FractionBox["1", "2"], + SuperscriptBox["t2", "2"], " ", "a1"}], "+", + RowBox[{ + FractionBox["1", "6"], + SuperscriptBox["t2", "3"], + RowBox[{"jerkProfile", "[", + RowBox[{"[", "2", "]"}], "]"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"p3", "=", + RowBox[{"p2", "+", + RowBox[{"t3", " ", "v2"}], "+", + RowBox[{ + FractionBox["1", "2"], + SuperscriptBox["t3", "2"], " ", "a2"}], "+", + RowBox[{ + FractionBox["1", "6"], + SuperscriptBox["t3", "3"], + RowBox[{"jerkProfile", "[", + RowBox[{"[", "3", "]"}], "]"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"p4", "=", + RowBox[{"p3", "+", + RowBox[{"t4", " ", "v3"}], "+", + RowBox[{ + FractionBox["1", "2"], + SuperscriptBox["t4", "2"], " ", "a3"}], "+", + RowBox[{ + FractionBox["1", "6"], + SuperscriptBox["t4", "3"], + RowBox[{"jerkProfile", "[", + RowBox[{"[", "4", "]"}], "]"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"p5", "=", + RowBox[{"p4", "+", + RowBox[{"t5", " ", "v4"}], "+", + RowBox[{ + FractionBox["1", "2"], + SuperscriptBox["t5", "2"], " ", "a4"}], "+", + RowBox[{ + FractionBox["1", "6"], + SuperscriptBox["t5", "3"], + RowBox[{"jerkProfile", "[", + RowBox[{"[", "5", "]"}], "]"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"p6", "=", + RowBox[{"p5", "+", + RowBox[{"t6", " ", "v5"}], "+", + RowBox[{ + FractionBox["1", "2"], + SuperscriptBox["t6", "2"], " ", "a5"}], "+", + RowBox[{ + FractionBox["1", "6"], + SuperscriptBox["t6", "3"], + RowBox[{"jerkProfile", "[", + RowBox[{"[", "6", "]"}], "]"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"p7", "=", + RowBox[{"p6", "+", + RowBox[{"t7", " ", "v6"}], "+", + RowBox[{ + FractionBox["1", "2"], + SuperscriptBox["t7", "2"], " ", "a6"}], "+", + RowBox[{ + FractionBox["1", "6"], + SuperscriptBox["t7", "3"], + RowBox[{"jerkProfile", "[", + RowBox[{"[", "7", "]"}], "]"}]}]}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"tAll", "=", + RowBox[{ + "t1", "+", "t2", "+", "t3", "+", "t4", "+", "t5", "+", "t6", "+", "t7"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"tVars", "=", + RowBox[{"{", + RowBox[{ + "t1", ",", "t2", ",", "t3", ",", "t4", ",", "t5", ",", "t6", ",", "t7"}], + "}"}]}], ";"}]}], "Input", + CellChangeTimes->{3.819602008191699*^9}, + CellLabel->"In[7]:=",ExpressionUUID->"099a8ff3-0e49-4a08-b964-3c54b7f9002d"], + +Cell[BoxData[""], "Input", + CellChangeTimes->{{3.819561618200523*^9, + 3.8195616479009237`*^9}},ExpressionUUID->"ab8196f4-4496-4301-8655-\ +17a49063bd9d"], + +Cell[CellGroupData[{ + +Cell[BoxData[{ + RowBox[{"tmp", "=", + RowBox[{"Simplify", "[", + RowBox[{ + RowBox[{ + RowBox[{"(", + RowBox[{"p7", "-", "pf"}], ")"}], "/.", + RowBox[{"{", + RowBox[{ + RowBox[{"t1", "\[Rule]", "0"}], ",", + RowBox[{"t2", "\[Rule]", "0"}], ",", + RowBox[{"t3", "\[Rule]", "t"}], ",", + RowBox[{"t6", "\[Rule]", "0"}], ",", + RowBox[{"t7", "\[Rule]", + RowBox[{"tf", "-", + RowBox[{"(", + RowBox[{"t", "+", "t4", "+", "t5"}], ")"}]}]}]}], "}"}]}], "/.", + RowBox[{"{", + RowBox[{ + RowBox[{"t4", "\[Rule]", + RowBox[{"(", + RowBox[{"tf", "-", + RowBox[{"2", "t"}], "-", + FractionBox[ + RowBox[{"(", + RowBox[{"af", "-", "a0"}], ")"}], "jMax"], "-", + RowBox[{ + RowBox[{"Sqrt", "[", + RowBox[{"(", + RowBox[{"2", "*", + RowBox[{"(", + RowBox[{ + SuperscriptBox["a0", "2"], "+", + SuperscriptBox["af", "2"], "+", + RowBox[{"2", "af", " ", "jMax", " ", "t"}], "-", + RowBox[{"2", "a0", + RowBox[{"(", + RowBox[{"af", "+", + RowBox[{"jMax", + RowBox[{"(", + RowBox[{"t", "-", "tf"}], ")"}]}]}], ")"}]}], "+", + RowBox[{"2", "jMax", + RowBox[{"(", + RowBox[{ + RowBox[{"jMax", " ", "t", + RowBox[{"(", + RowBox[{"t", "-", "tf"}], ")"}]}], "-", "vd"}], ")"}]}]}], + ")"}]}], ")"}], "]"}], "/", + RowBox[{"(", + RowBox[{"Abs", "[", "jMax", "]"}], ")"}]}]}], ")"}]}], ",", + RowBox[{"t5", "->", + RowBox[{ + RowBox[{"Sqrt", "[", + RowBox[{"(", + RowBox[{"2", "*", + RowBox[{"(", + RowBox[{ + SuperscriptBox["a0", "2"], "+", + SuperscriptBox["af", "2"], "+", + RowBox[{"2", "af", " ", "jMax", " ", "t"}], "-", + RowBox[{"2", "a0", + RowBox[{"(", + RowBox[{"af", "+", + RowBox[{"jMax", + RowBox[{"(", + RowBox[{"t", "-", "tf"}], ")"}]}]}], ")"}]}], "+", + RowBox[{"2", "jMax", + RowBox[{"(", + RowBox[{ + RowBox[{"jMax", " ", "t", + RowBox[{"(", + RowBox[{"t", "-", "tf"}], ")"}]}], "-", "vd"}], ")"}]}]}], + ")"}]}], ")"}], "]"}], "/", + RowBox[{"(", + RowBox[{"2", + RowBox[{"Abs", "[", "jMax", "]"}]}], ")"}]}]}]}], "}"}]}], + "]"}]}], "\[IndentingNewLine]", + RowBox[{"ToString", "[", + RowBox[{ + RowBox[{"Simplify", "[", "tmp", "]"}], ",", "CForm"}], + "]"}], "\[IndentingNewLine]", + RowBox[{"CopyToClipboard", "[", "%", "]"}]}], "Input", + CellChangeTimes->{{3.81956156377069*^9, 3.819561609073489*^9}, { + 3.8195616490010757`*^9, 3.819561880668848*^9}, {3.8195620220367327`*^9, + 3.819562023947276*^9}, {3.8195621217573977`*^9, 3.8195621358064003`*^9}, { + 3.819562230757843*^9, 3.819562249667959*^9}}, + CellLabel->"In[98]:=",ExpressionUUID->"58da9551-5167-44e7-a65b-195023eb5f56"], + +Cell[BoxData[ + RowBox[{ + RowBox[{ + FractionBox["1", + RowBox[{"6", " ", + SuperscriptBox["jMax", "2"]}]], + RowBox[{"(", + RowBox[{ + RowBox[{"-", + SuperscriptBox["a0", "3"]}], "+", + SuperscriptBox["af", "3"], "+", + RowBox[{"3", " ", + SuperscriptBox["af", "2"], " ", "jMax", " ", "t"}], "+", + RowBox[{"3", " ", "af", " ", + SuperscriptBox["jMax", "2"], " ", + SuperscriptBox["t", "2"]}], "+", + RowBox[{"3", " ", + SuperscriptBox["a0", "2"], " ", + RowBox[{"(", + RowBox[{"af", "+", + RowBox[{"jMax", " ", "t"}]}], ")"}]}], "-", + RowBox[{"3", " ", "a0", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["af", "2"], "+", + RowBox[{"2", " ", "af", " ", "jMax", " ", "t"}], "+", + RowBox[{ + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["t", "2"], "-", + SuperscriptBox["tf", "2"]}], ")"}]}]}], ")"}]}], "+", + RowBox[{"3", " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "p0"}], "-", + RowBox[{"2", " ", "pf"}], "+", + RowBox[{"jMax", " ", "t", " ", + RowBox[{"(", + RowBox[{"t", "-", "tf"}], ")"}], " ", "tf"}], "+", + RowBox[{"2", " ", "tf", " ", "v0"}]}], ")"}]}]}], ")"}]}], "-", + FractionBox[ + RowBox[{"jMax", " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["a0", "2"], "+", + SuperscriptBox["af", "2"], "+", + RowBox[{"2", " ", "af", " ", "jMax", " ", "t"}], "-", + RowBox[{"2", " ", "a0", " ", + RowBox[{"(", + RowBox[{"af", "+", + RowBox[{"jMax", " ", + RowBox[{"(", + RowBox[{"t", "-", "tf"}], ")"}]}]}], ")"}]}], "+", + RowBox[{"2", " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"jMax", " ", "t", " ", + RowBox[{"(", + RowBox[{"t", "-", "tf"}], ")"}]}], "-", "vd"}], ")"}]}]}], ")"}], + + RowBox[{"3", "/", "2"}]]}], + RowBox[{"2", " ", + SqrtBox["2"], " ", + SuperscriptBox[ + RowBox[{"Abs", "[", "jMax", "]"}], "3"]}]], "+", + FractionBox[ + RowBox[{ + RowBox[{"(", + RowBox[{"a0", "-", "af", "-", + RowBox[{"jMax", " ", "t"}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["a0", "2"], "+", + SuperscriptBox["af", "2"], "+", + RowBox[{"2", " ", "af", " ", "jMax", " ", "t"}], "-", + RowBox[{"2", " ", "a0", " ", + RowBox[{"(", + RowBox[{"af", "+", + RowBox[{"jMax", " ", + RowBox[{"(", + RowBox[{"t", "-", "tf"}], ")"}]}]}], ")"}]}], "+", + RowBox[{"2", " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"jMax", " ", "t", " ", + RowBox[{"(", + RowBox[{"t", "-", "tf"}], ")"}]}], "-", "vd"}], ")"}]}]}], ")"}]}], + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"Abs", "[", "jMax", "]"}], "2"]}]]}]], "Output", + CellChangeTimes->{{3.819561649749753*^9, 3.8195616568809767`*^9}, { + 3.819561697059889*^9, 3.819561735670363*^9}, 3.8195617682059298`*^9, { + 3.81956180497314*^9, 3.819561881074012*^9}, {3.819562016223662*^9, + 3.819562024427198*^9}, {3.8195621277446213`*^9, 3.819562136285375*^9}, { + 3.8195622332578373`*^9, 3.819562251052745*^9}}, + CellLabel->"Out[98]=",ExpressionUUID->"6db2ca46-5d56-4204-a972-d02ff6728af7"], + +Cell[BoxData["\<\"(-Power(a0,3) + Power(af,3) + 3*Power(af,2)*jMax*t + \ +3*af*Power(jMax,2)*Power(t,2) + 3*Power(a0,2)*(af + jMax*t) - \ +3*a0*(Power(af,2) + 2*af*jMax*t + Power(jMax,2)*(Power(t,2) - Power(tf,2))) + \ +3*Power(jMax,2)*(2*p0 - 2*pf + jMax*t*(t - tf)*tf + \ +2*tf*v0))/(6.*Power(jMax,2)) - (jMax*Power(Power(a0,2) + Power(af,2) + \ +2*af*jMax*t - 2*a0*(af + jMax*(t - tf)) + 2*jMax*(jMax*t*(t - tf) - \ +vd),1.5))/(2.*Sqrt(2)*Power(Abs(jMax),3)) + ((a0 - af - jMax*t)*(Power(a0,2) \ ++ Power(af,2) + 2*af*jMax*t - 2*a0*(af + jMax*(t - tf)) + 2*jMax*(jMax*t*(t - \ +tf) - vd)))/(2.*Power(Abs(jMax),2))\"\>"], "Output", + CellChangeTimes->{{3.819561649749753*^9, 3.8195616568809767`*^9}, { + 3.819561697059889*^9, 3.819561735670363*^9}, 3.8195617682059298`*^9, { + 3.81956180497314*^9, 3.819561881074012*^9}, {3.819562016223662*^9, + 3.819562024427198*^9}, {3.8195621277446213`*^9, 3.819562136285375*^9}, { + 3.8195622332578373`*^9, 3.819562251091281*^9}}, + CellLabel->"Out[99]=",ExpressionUUID->"a5342eb2-cf4b-4562-a3c9-4043e660ac51"] +}, Open ]] +}, +WindowSize->{808, 747}, +WindowMargins->{{Automatic, 139}, {Automatic, 101}}, +FrontEndVersion->"12.1 for Mac OS X x86 (64-bit) (June 19, 2020)", +StyleDefinitions->"Default.nb", +ExpressionUUID->"fa4eca22-4fcd-44eb-91d3-fdc0150e5310" +] +(* End of Notebook Content *) + +(* Internal cache information *) +(*CellTagsOutline +CellTagsIndex->{} +*) +(*CellTagsIndex +CellTagsIndex->{} +*) +(*NotebookFileOutline +Notebook[{ +Cell[558, 20, 7089, 227, 919, "Input",ExpressionUUID->"099a8ff3-0e49-4a08-b964-3c54b7f9002d"], +Cell[7650, 249, 154, 3, 30, "Input",ExpressionUUID->"ab8196f4-4496-4301-8655-17a49063bd9d"], +Cell[CellGroupData[{ +Cell[7829, 256, 3250, 86, 307, "Input",ExpressionUUID->"58da9551-5167-44e7-a65b-195023eb5f56"], +Cell[11082, 344, 3457, 98, 178, "Output",ExpressionUUID->"6db2ca46-5d56-4204-a972-d02ff6728af7"], +Cell[14542, 444, 1045, 14, 182, "Output",ExpressionUUID->"a5342eb2-cf4b-4562-a3c9-4043e660ac51"] +}, Open ]] +} +] +*) + diff --git a/notebooks/ruckig-step2.nb b/notebooks/ruckig-step2.nb index 8a54edd1..44dc8d0a 100644 --- a/notebooks/ruckig-step2.nb +++ b/notebooks/ruckig-step2.nb @@ -10,10 +10,10 @@ NotebookFileLineBreakTest NotebookFileLineBreakTest NotebookDataPosition[ 158, 7] -NotebookDataLength[ 109889, 2897] -NotebookOptionsPosition[ 104046, 2794] -NotebookOutlinePosition[ 104470, 2811] -CellTagsIndexPosition[ 104427, 2808] +NotebookDataLength[ 109915, 2898] +NotebookOptionsPosition[ 104073, 2795] +NotebookOutlinePosition[ 104496, 2812] +CellTagsIndexPosition[ 104453, 2809] WindowFrame->Normal*) (* Beginning of Notebook Content *) @@ -262,7 +262,7 @@ Cell[BoxData[{ 3.817577983155306*^9, 3.817610819742194*^9, 3.819359229321691*^9, 3.819359611425458*^9, {3.819359641697466*^9, 3.8193596426416473`*^9}}, CellLabel-> - "In[122]:=",ExpressionUUID->"9382707b-2876-460d-b0fb-ca70e05b1e9a"], + "In[103]:=",ExpressionUUID->"9382707b-2876-460d-b0fb-ca70e05b1e9a"], Cell[CellGroupData[{ @@ -583,12 +583,13 @@ Cell[BoxData[ 3.8176487661502733`*^9, 3.81764877682362*^9}, {3.817648986736754*^9, 3.817649005091858*^9}, 3.817649060563797*^9}, CellLabel-> - "In[114]:=",ExpressionUUID->"93a49805-7b39-4e71-a5ce-78f2b77ae9c0"], + "In[129]:=",ExpressionUUID->"93a49805-7b39-4e71-a5ce-78f2b77ae9c0"], Cell[BoxData["$Aborted"], "Output", - CellChangeTimes->{3.819359328459915*^9, 3.819359403231781*^9}, + CellChangeTimes->{3.819359328459915*^9, 3.819359403231781*^9, + 3.8195631130890636`*^9}, CellLabel-> - "Out[114]=",ExpressionUUID->"6ace0860-a564-45c6-a13c-853aef9b6204"] + "Out[129]=",ExpressionUUID->"7022ff95-9f21-46fd-84d4-9abc7a83743a"] }, Open ]], Cell["\<\ @@ -2793,7 +2794,7 @@ Cell[BoxData[""], "Input", e86d60821dfb"] }, WindowSize->{979, 784}, -WindowMargins->{{119, Automatic}, {34, Automatic}}, +WindowMargins->{{Automatic, 301}, {Automatic, 0}}, Magnification:>0.9 Inherited, FrontEndVersion->"12.1 for Mac OS X x86 (64-bit) (June 19, 2020)", StyleDefinitions->"Default.nb", @@ -2814,91 +2815,91 @@ Cell[558, 20, 230, 4, 31, "Text",ExpressionUUID->"039b787c-64fd-4dfb-8011-578c38 Cell[791, 26, 7842, 238, 822, "Input",ExpressionUUID->"9382707b-2876-460d-b0fb-ca70e05b1e9a"], Cell[CellGroupData[{ Cell[8658, 268, 12691, 317, 408, "Input",ExpressionUUID->"93a49805-7b39-4e71-a5ce-78f2b77ae9c0"], -Cell[21352, 587, 182, 3, 31, "Output",ExpressionUUID->"6ace0860-a564-45c6-a13c-853aef9b6204"] +Cell[21352, 587, 209, 4, 31, "Output",ExpressionUUID->"7022ff95-9f21-46fd-84d4-9abc7a83743a"] }, Open ]], -Cell[21549, 593, 1021, 23, 259, "Text",ExpressionUUID->"0616ae41-3a02-473a-878f-96412e2fc8c5"], -Cell[22573, 618, 284, 7, 52, "Text",ExpressionUUID->"407acd61-eed1-4280-b150-0da33c32906a"], +Cell[21576, 594, 1021, 23, 259, "Text",ExpressionUUID->"0616ae41-3a02-473a-878f-96412e2fc8c5"], +Cell[22600, 619, 284, 7, 52, "Text",ExpressionUUID->"407acd61-eed1-4280-b150-0da33c32906a"], Cell[CellGroupData[{ -Cell[22882, 629, 1927, 32, 66, "Input",ExpressionUUID->"f08feb2e-6079-473b-b866-92670dea0a56"], -Cell[24812, 663, 1359, 19, 31, "Output",ExpressionUUID->"6ba93a6f-1693-4cd0-bd08-3827239d3713"] +Cell[22909, 630, 1927, 32, 66, "Input",ExpressionUUID->"f08feb2e-6079-473b-b866-92670dea0a56"], +Cell[24839, 664, 1359, 19, 31, "Output",ExpressionUUID->"6ba93a6f-1693-4cd0-bd08-3827239d3713"] }, Open ]], -Cell[26186, 685, 247, 4, 31, "Text",ExpressionUUID->"f1684154-b22e-4827-98da-4e4dad150835"], +Cell[26213, 686, 247, 4, 31, "Text",ExpressionUUID->"f1684154-b22e-4827-98da-4e4dad150835"], Cell[CellGroupData[{ -Cell[26458, 693, 2231, 55, 163, "Input",ExpressionUUID->"217d7885-1018-483e-8952-08082feb5695"], -Cell[28692, 750, 4547, 121, 141, "Output",ExpressionUUID->"0add838c-dd53-4b4a-9a83-667a65b6c2dc"] +Cell[26485, 694, 2231, 55, 163, "Input",ExpressionUUID->"217d7885-1018-483e-8952-08082feb5695"], +Cell[28719, 751, 4547, 121, 141, "Output",ExpressionUUID->"0add838c-dd53-4b4a-9a83-667a65b6c2dc"] }, Open ]], Cell[CellGroupData[{ -Cell[33276, 876, 1377, 28, 85, "Input",ExpressionUUID->"58c30e25-83c4-4d05-ad21-e88fcb7f4181"], -Cell[34656, 906, 862, 14, 49, "Output",ExpressionUUID->"28ef38fa-fff6-4035-92dc-49a30c5e7739"] +Cell[33303, 877, 1377, 28, 85, "Input",ExpressionUUID->"58c30e25-83c4-4d05-ad21-e88fcb7f4181"], +Cell[34683, 907, 862, 14, 49, "Output",ExpressionUUID->"28ef38fa-fff6-4035-92dc-49a30c5e7739"] }, Open ]], -Cell[35533, 923, 229, 4, 31, "Text",ExpressionUUID->"5c00d19c-efbb-475f-a4a4-adc1877059e7"], -Cell[35765, 929, 5874, 132, 275, "Input",ExpressionUUID->"013ba19d-162c-4e14-9e6b-43bcf125ceef"], -Cell[41642, 1063, 869, 20, 218, "Text",ExpressionUUID->"ccf44efd-e09a-4bbb-a696-b4c90f7a95eb"], +Cell[35560, 924, 229, 4, 31, "Text",ExpressionUUID->"5c00d19c-efbb-475f-a4a4-adc1877059e7"], +Cell[35792, 930, 5874, 132, 275, "Input",ExpressionUUID->"013ba19d-162c-4e14-9e6b-43bcf125ceef"], +Cell[41669, 1064, 869, 20, 218, "Text",ExpressionUUID->"ccf44efd-e09a-4bbb-a696-b4c90f7a95eb"], Cell[CellGroupData[{ -Cell[42536, 1087, 838, 19, 66, "Input",ExpressionUUID->"b4fd3303-7c30-4af8-94b1-4505577eaf40"], -Cell[43377, 1108, 5908, 138, 161, "Output",ExpressionUUID->"e653eb3d-f560-4f2c-9646-ccf546b5417c"] +Cell[42563, 1088, 838, 19, 66, "Input",ExpressionUUID->"b4fd3303-7c30-4af8-94b1-4505577eaf40"], +Cell[43404, 1109, 5908, 138, 161, "Output",ExpressionUUID->"e653eb3d-f560-4f2c-9646-ccf546b5417c"] }, Open ]], -Cell[49300, 1249, 204, 4, 31, "Text",ExpressionUUID->"6e1bb727-a1dc-4c24-af7f-abfa0f3b9409"], +Cell[49327, 1250, 204, 4, 31, "Text",ExpressionUUID->"6e1bb727-a1dc-4c24-af7f-abfa0f3b9409"], Cell[CellGroupData[{ -Cell[49529, 1257, 1898, 46, 144, "Input",ExpressionUUID->"b4d0a35e-2a8e-482f-b0a5-593ccb90a6dd"], -Cell[51430, 1305, 7831, 209, 189, "Output",ExpressionUUID->"c74db8a1-1f65-4bfc-962e-fffe0d610493"] +Cell[49556, 1258, 1898, 46, 144, "Input",ExpressionUUID->"b4d0a35e-2a8e-482f-b0a5-593ccb90a6dd"], +Cell[51457, 1306, 7831, 209, 189, "Output",ExpressionUUID->"c74db8a1-1f65-4bfc-962e-fffe0d610493"] }, Open ]], Cell[CellGroupData[{ -Cell[59298, 1519, 1044, 23, 85, "Input",ExpressionUUID->"111d8f11-4472-4521-8634-15b568e443ee"], -Cell[60345, 1544, 4861, 138, 123, "Output",ExpressionUUID->"6ac34b7e-fe01-4d94-8ecc-37e88505a8af"] +Cell[59325, 1520, 1044, 23, 85, "Input",ExpressionUUID->"111d8f11-4472-4521-8634-15b568e443ee"], +Cell[60372, 1545, 4861, 138, 123, "Output",ExpressionUUID->"6ac34b7e-fe01-4d94-8ecc-37e88505a8af"] }, Open ]], Cell[CellGroupData[{ -Cell[65243, 1687, 1812, 44, 116, "Input",ExpressionUUID->"0d5d8425-f226-4e6a-91fd-34a5822f73b9"], -Cell[67058, 1733, 2994, 80, 155, "Output",ExpressionUUID->"1dcd2096-117e-4fa7-8a27-82d21901823d"] +Cell[65270, 1688, 1812, 44, 116, "Input",ExpressionUUID->"0d5d8425-f226-4e6a-91fd-34a5822f73b9"], +Cell[67085, 1734, 2994, 80, 155, "Output",ExpressionUUID->"1dcd2096-117e-4fa7-8a27-82d21901823d"] }, Open ]], Cell[CellGroupData[{ -Cell[70089, 1818, 764, 21, 46, "Input",ExpressionUUID->"4f3c940a-b41f-43f3-9aa8-8115c96b6bee"], -Cell[70856, 1841, 2830, 70, 76, "Output",ExpressionUUID->"f8a67d4a-9073-4868-948b-275b14bd35f8"] +Cell[70116, 1819, 764, 21, 46, "Input",ExpressionUUID->"4f3c940a-b41f-43f3-9aa8-8115c96b6bee"], +Cell[70883, 1842, 2830, 70, 76, "Output",ExpressionUUID->"f8a67d4a-9073-4868-948b-275b14bd35f8"] }, Open ]], -Cell[73701, 1914, 472, 14, 42, "Input",ExpressionUUID->"ac4e2179-0486-40d4-a93e-0bd772b55249"], -Cell[74176, 1930, 190, 3, 31, "Text",ExpressionUUID->"56699e4e-5d18-41ce-a54a-bd05638d5d2d"], +Cell[73728, 1915, 472, 14, 42, "Input",ExpressionUUID->"ac4e2179-0486-40d4-a93e-0bd772b55249"], +Cell[74203, 1931, 190, 3, 31, "Text",ExpressionUUID->"56699e4e-5d18-41ce-a54a-bd05638d5d2d"], Cell[CellGroupData[{ -Cell[74391, 1937, 895, 24, 42, "Input",ExpressionUUID->"5e6a25cd-7819-4650-a7d0-08f771745532"], -Cell[75289, 1963, 561, 15, 35, "Output",ExpressionUUID->"3d5e1dff-06d5-4a78-8211-d677f7d95c06"] +Cell[74418, 1938, 895, 24, 42, "Input",ExpressionUUID->"5e6a25cd-7819-4650-a7d0-08f771745532"], +Cell[75316, 1964, 561, 15, 35, "Output",ExpressionUUID->"3d5e1dff-06d5-4a78-8211-d677f7d95c06"] }, Open ]], Cell[CellGroupData[{ -Cell[75887, 1983, 1011, 27, 27, "Input",ExpressionUUID->"02c4da17-fbe6-4543-b195-45be4ca9b841"], -Cell[76901, 2012, 4330, 118, 169, "Output",ExpressionUUID->"17ffb416-f6fc-47ba-8ab0-4b6dd6842181"] +Cell[75914, 1984, 1011, 27, 27, "Input",ExpressionUUID->"02c4da17-fbe6-4543-b195-45be4ca9b841"], +Cell[76928, 2013, 4330, 118, 169, "Output",ExpressionUUID->"17ffb416-f6fc-47ba-8ab0-4b6dd6842181"] }, Open ]], Cell[CellGroupData[{ -Cell[81268, 2135, 619, 15, 66, "Input",ExpressionUUID->"7670aa61-4700-4432-8cc7-2b72034ed478"], -Cell[81890, 2152, 1008, 28, 50, "Output",ExpressionUUID->"f9a3e9af-66df-4068-9321-85fb8bf641e1"] +Cell[81295, 2136, 619, 15, 66, "Input",ExpressionUUID->"7670aa61-4700-4432-8cc7-2b72034ed478"], +Cell[81917, 2153, 1008, 28, 50, "Output",ExpressionUUID->"f9a3e9af-66df-4068-9321-85fb8bf641e1"] }, Open ]], Cell[CellGroupData[{ -Cell[82935, 2185, 608, 17, 46, "Input",ExpressionUUID->"c3eac5a8-2fa9-4c22-9d0b-ae3fc680cf99"], -Cell[83546, 2204, 691, 19, 54, "Output",ExpressionUUID->"4dc6f8bf-3425-4ace-bcc0-7ad3cf515abf"] +Cell[82962, 2186, 608, 17, 46, "Input",ExpressionUUID->"c3eac5a8-2fa9-4c22-9d0b-ae3fc680cf99"], +Cell[83573, 2205, 691, 19, 54, "Output",ExpressionUUID->"4dc6f8bf-3425-4ace-bcc0-7ad3cf515abf"] }, Open ]], -Cell[84252, 2226, 234, 4, 31, "Text",ExpressionUUID->"f8c02129-5c08-4287-9ce4-9d2249b49a47"], +Cell[84279, 2227, 234, 4, 31, "Text",ExpressionUUID->"f8c02129-5c08-4287-9ce4-9d2249b49a47"], Cell[CellGroupData[{ -Cell[84511, 2234, 1930, 48, 106, "Input",ExpressionUUID->"60805ad3-28dc-4b78-ba4e-f0602908471c"], -Cell[86444, 2284, 2173, 53, 92, "Output",ExpressionUUID->"c5369eac-c19b-4faa-8087-85202ad8609e"] +Cell[84538, 2235, 1930, 48, 106, "Input",ExpressionUUID->"60805ad3-28dc-4b78-ba4e-f0602908471c"], +Cell[86471, 2285, 2173, 53, 92, "Output",ExpressionUUID->"c5369eac-c19b-4faa-8087-85202ad8609e"] }, Open ]], Cell[CellGroupData[{ -Cell[88654, 2342, 448, 11, 47, "Input",ExpressionUUID->"1adf51d5-6fe8-4769-923d-21e404dcac21"], -Cell[89105, 2355, 290, 4, 31, "Output",ExpressionUUID->"bf241e25-201f-42dc-ae02-700d684d147e"] +Cell[88681, 2343, 448, 11, 47, "Input",ExpressionUUID->"1adf51d5-6fe8-4769-923d-21e404dcac21"], +Cell[89132, 2356, 290, 4, 31, "Output",ExpressionUUID->"bf241e25-201f-42dc-ae02-700d684d147e"] }, Open ]], Cell[CellGroupData[{ -Cell[89432, 2364, 786, 19, 66, "Input",ExpressionUUID->"921d5f93-0895-485d-9d3c-e459a83a8814"], -Cell[90221, 2385, 5743, 154, 317, "Output",ExpressionUUID->"0b148223-b092-480e-ba11-e425965fccb8"] +Cell[89459, 2365, 786, 19, 66, "Input",ExpressionUUID->"921d5f93-0895-485d-9d3c-e459a83a8814"], +Cell[90248, 2386, 5743, 154, 317, "Output",ExpressionUUID->"0b148223-b092-480e-ba11-e425965fccb8"] }, Open ]], Cell[CellGroupData[{ -Cell[96001, 2544, 569, 15, 66, "Input",ExpressionUUID->"9038a11c-84fb-477d-abca-0d7fed1418f2"], -Cell[96573, 2561, 2231, 65, 159, "Output",ExpressionUUID->"3dfbcaea-61a7-48dc-8700-3600d97979e7"] +Cell[96028, 2545, 569, 15, 66, "Input",ExpressionUUID->"9038a11c-84fb-477d-abca-0d7fed1418f2"], +Cell[96600, 2562, 2231, 65, 159, "Output",ExpressionUUID->"3dfbcaea-61a7-48dc-8700-3600d97979e7"] }, Open ]], Cell[CellGroupData[{ -Cell[98841, 2631, 1432, 39, 144, "Input",ExpressionUUID->"50f38d09-add8-4aaa-80cf-82c2c2960cb5"], -Cell[100276, 2672, 936, 26, 54, "Output",ExpressionUUID->"ba57fae9-f30c-4730-8996-2fb86edb2fca"] +Cell[98868, 2632, 1432, 39, 144, "Input",ExpressionUUID->"50f38d09-add8-4aaa-80cf-82c2c2960cb5"], +Cell[100303, 2673, 936, 26, 54, "Output",ExpressionUUID->"ba57fae9-f30c-4730-8996-2fb86edb2fca"] }, Open ]], Cell[CellGroupData[{ -Cell[101249, 2703, 946, 26, 42, "Input",ExpressionUUID->"ecc98e15-0b26-499a-88a0-118989f052e9"], -Cell[102198, 2731, 1677, 55, 121, "Output",ExpressionUUID->"55efec97-7768-49b4-945b-81d8a50e7a37"] +Cell[101276, 2704, 946, 26, 42, "Input",ExpressionUUID->"ecc98e15-0b26-499a-88a0-118989f052e9"], +Cell[102225, 2732, 1677, 55, 121, "Output",ExpressionUUID->"55efec97-7768-49b4-945b-81d8a50e7a37"] }, Open ]], -Cell[103890, 2789, 152, 3, 27, "Input",ExpressionUUID->"f9662b36-78d8-4212-a221-e86d60821dfb"] +Cell[103917, 2790, 152, 3, 27, "Input",ExpressionUUID->"f9662b36-78d8-4212-a221-e86d60821dfb"] } ] *) diff --git a/src/step1.cpp b/src/step1.cpp index 3629d8af..13d26ac8 100644 --- a/src/step1.cpp +++ b/src/step1.cpp @@ -7,7 +7,6 @@ namespace ruckig { Step1::Step1(double p0, double v0, double a0, double pf, double vf, double af, double vMax, double vMin, double aMax, double jMax): p0(p0), v0(v0), a0(a0), pf(pf), vf(vf), af(af), vMax(vMax), vMin(vMin), aMax(aMax), jMax(jMax) { - // max values needs to be invariant to plus minus sign change pd = pf - p0; v0_v0 = v0 * v0; @@ -15,7 +14,6 @@ Step1::Step1(double p0, double v0, double a0, double pf, double vf, double af, d a0_a0 = a0 * a0; af_af = af * af; - aMax_aMax = aMax * aMax; a0_p3 = a0 * a0_a0; a0_p4 = a0_a0 * a0_a0; @@ -26,6 +24,8 @@ Step1::Step1(double p0, double v0, double a0, double pf, double vf, double af, d af_p5 = af_p3 * af_af; af_p6 = af_p4 * af_af; + // max values needs to be invariant to plus minus sign change + aMax_aMax = aMax * aMax; jMax_jMax = jMax * jMax; } @@ -87,10 +87,10 @@ void Step1::time_up_vel(Profile& profile, double vMax, double aMax, double jMax) const double h2 = Sqrt(a0_a0/2 + jMax*(vMax - v0))/Abs(jMax); // Solution 3/4 - profile.t[0] = (-a0 + h2*jMax)/jMax; + profile.t[0] = h2 - a0/jMax; profile.t[1] = 0; - profile.t[2] = profile.t[0] + a0/jMax; - profile.t[3] = (2*(af_p3 - a0_p3) + 6*jMax*(a0*v0 - af*vf) + 3*af_af*h1*jMax + 3*jMax_jMax*(h2*(a0_a0/jMax - 2*(v0 + vMax)) + 2*pd - 2*(vf + vMax)*h1))/(6*jMax_jMax*vMax); + profile.t[2] = h2; + profile.t[3] = (af_p3 - a0_p3)/(3*jMax_jMax*vMax) + (a0*v0 - af*vf + (af_af*h1 + a0_a0*h2)/2)/(jMax*vMax) - (v0/vMax + 1.0)*h2 - (vf/vMax + 1.0)*h1 + pd/vMax; profile.t[4] = h1; profile.t[5] = 0; profile.t[6] = profile.t[4] + af/jMax; @@ -306,7 +306,7 @@ void Step1::time_up_none(Profile& profile, double vMax, double aMax, double jMax } // Single Newton step (regarding polynom) - if (std::abs(Roots::polyEval(polynom, t)) > 1e-14) { + if (std::abs(Roots::polyEval(polynom, t)) > Roots::tolerance) { t -= Roots::polyEval(polynom, t) / Roots::polyEval(Roots::polyDeri(polynom), t); } @@ -358,8 +358,15 @@ void Step1::time_up_none(Profile& profile, double vMax, double aMax, double jMax deriv[4] = 2./6 * polynom[4]; deriv[5] = 1./6 * polynom[5]; - auto dd_extremas = Roots::solveQuartMonic(4./5 * deriv[1], 3./5 * deriv[2], 2./5 * deriv[3], 1./5 * deriv[4]); - std::set> dd_tz_intervals; + std::array dderiv; + dderiv[0] = 1.0; + dderiv[1] = 4./5 * deriv[1]; + dderiv[2] = 3./5 * deriv[2]; + dderiv[3] = 2./5 * deriv[3]; + dderiv[4] = 1./5 * deriv[4]; + + auto dd_extremas = Roots::solveQuartMonic(dderiv); + std::set> dd_tz_intervals; double tz_min {0.0}; double tz_max = {1e5}; @@ -370,7 +377,10 @@ void Step1::time_up_none(Profile& profile, double vMax, double aMax, double jMax continue; } - // Check that polynom(lower) and polynom(upper) have different signs (should only happen at first and last boundary) + // if (std::abs(Roots::polyEval(dderiv, tz)) > 1e-14) { + // tz -= Roots::polyEval(dderiv, tz) / Roots::polyEval(Roots::polyDeri(dderiv), tz); + // } + if (Roots::polyEval(deriv, dd_tz_current) * Roots::polyEval(deriv, tz) < 0) { dd_tz_intervals.insert({dd_tz_current, tz}); } @@ -380,33 +390,26 @@ void Step1::time_up_none(Profile& profile, double vMax, double aMax, double jMax dd_tz_intervals.insert({dd_tz_current, tz_max}); } - std::set> tz_intervals; + std::set roots; double tz_current {tz_min}; for (auto interval: dd_tz_intervals) { - double lower = std::get<0>(interval); - double upper = std::get<1>(interval); - double tz = Roots::shrinkInterval(deriv, lower, upper); + double tz = Roots::shrinkInterval(deriv, interval.first, interval.second); if (tz <= 0.0) { continue; } - // Check that polynom(lower) and polynom(upper) have different signs (should only happen at first and last boundary) + if (Roots::polyEval(polynom, tz_current) * Roots::polyEval(polynom, tz) < 0) { - tz_intervals.insert({tz_current, tz}); + roots.insert(Roots::shrinkInterval(polynom, tz_current, tz)); } tz_current = tz; } if (Roots::polyEval(polynom, tz_current) * Roots::polyEval(polynom, tz_max) < 0) { - tz_intervals.insert({tz_current, tz_max}); + roots.insert(Roots::shrinkInterval(polynom, tz_current, tz_max)); } - for (auto interval: tz_intervals) { - // Use safe Newton method - double lower = std::get<0>(interval); - double upper = std::get<1>(interval); - double t = Roots::shrinkInterval(polynom, lower, upper); - + for (double t: roots) { if (t < 0.0) { continue; } diff --git a/src/step2.cpp b/src/step2.cpp index 63e9998c..47b47da9 100644 --- a/src/step2.cpp +++ b/src/step2.cpp @@ -259,9 +259,9 @@ bool Step2::time_up_vel(Profile& profile, double vMax, double aMax, double jMax) deriv[4] = 1./5 * polynom[4]; // Solve 4th order derivative analytically - auto extremas = Roots::solveQuartMonic(deriv[1], deriv[2], deriv[3], deriv[4]); - std::set> tz_intervals; - + auto extremas = Roots::solveQuartMonic(deriv); + + std::set roots; double tz_min {0.0}; double tz_max = std::min(tf, (tf - a0/jMax) / 2); double tz_current {tz_min}; @@ -271,49 +271,42 @@ bool Step2::time_up_vel(Profile& profile, double vMax, double aMax, double jMax) continue; } - // Check that polynom(lower) and polynom(upper) have different signs (should only happen at first and last boundary) - double val_current = Roots::polyEval(polynom, tz_current); - double val_new = Roots::polyEval(polynom, tz); - // std::cout << "tz: " << tz << " " << val_new << " " << val_current << std::endl; - if (std::abs(val_new) < 1e-15) { - tz_intervals.insert({tz - 2e-16, tz + 2e-16}); - tz += 1e-15; + const double res = std::abs(Roots::polyEval(Roots::polyDeri(deriv), tz)) * Roots::tolerance; + const double val_new = Roots::polyEval(polynom, tz); + if (std::abs(val_new) < res) { + roots.insert(tz); - } else if (val_current * val_new < 0) { - tz_intervals.insert({tz_current, tz}); + } else if (Roots::polyEval(polynom, tz_current) * val_new < 0) { + roots.insert(Roots::shrinkInterval(polynom, tz_current, tz)); } tz_current = tz; } if (Roots::polyEval(polynom, tz_current) * Roots::polyEval(polynom, tz_max) < 0) { - tz_intervals.insert({tz_current, tz_max}); + roots.insert(Roots::shrinkInterval(polynom, tz_current, tz_max)); } - for (auto interval: tz_intervals) { - // Use safe Newton method - const double lower = std::get<0>(interval); - const double upper = std::get<1>(interval); - double t = Roots::shrinkInterval(polynom, lower, upper); - + for (double t: roots) { // Single Newton step (regarding pd) { - const double h2 = Sqrt(2*(a0_a0 + af_af + 4*a0*jMax*t + 2*jMax*(jMax*t*t - vd)))/Abs(jMax); - const double orig = -pd - (2*a0_p3 + 4*af_p3 + 24*a0*jMax*t*(af + jMax*(t - tf) + jMax*h2/2) + 6*a0_a0*(af + jMax*(2*t - tf) + jMax*h2/2) + 6*af_af*jMax*h2/2 + 12*af*jMax*(jMax*t*t - vd) + 12*jMax_jMax*(jMax*t*t*(t - tf) - tf*v0 - h2/2*(vd - jMax*t*t)))/(12*jMax_jMax); - const double deriv = -(a0 + jMax*t)*(3*((a0_a0 + af_af) + 2*jMax_jMax*t*t + 2*jMax*(2*a0*t - vd))/(h2*jMax_jMax) + (a0 + 2*af)/jMax + (3*t - 2*tf)); - + const double h2 = Sqrt((a0_a0 + af_af)/2 + jMax*(2*a0*t + jMax*t*t - vd))/Abs(jMax); + const double orig = -pd - (2*a0_p3 + 4*af_p3 + 24*a0*jMax*t*(af + jMax*(t - tf) + jMax*h2) + 6*a0_a0*(af + jMax*(2*t - tf) + jMax*h2) + 6*af_af*jMax*h2 + 12*af*jMax*(jMax*t*t - vd) + 12*jMax_jMax*(jMax*t*t*(t - tf) - tf*v0 - h2*(vd - jMax*t*t)))/(12*jMax_jMax); + const double deriv = -(a0 + jMax*t)*(3*h2 + (a0 + 2*af)/jMax + (3*t - 2*tf)); t -= orig / deriv; } if (t < 0.0 || t > tf) { continue; } + + const double h1 = Sqrt((a0_a0 + af_af)/2 + jMax*(2*a0*t + jMax*t*t - vd))/Abs(jMax); profile.t[0] = t; profile.t[1] = 0; profile.t[2] = profile.t[0] + a0/jMax; - profile.t[4] = Sqrt(a0_a0/2 + af_af/2 + jMax*(2*a0*t + jMax*t*t - vd))/Abs(jMax); + profile.t[3] = tf - 2*(t + h1) - (a0 + af)/jMax; + profile.t[4] = h1; profile.t[5] = 0; profile.t[6] = profile.t[4] + af/jMax; - profile.t[3] = tf - (profile.t[0] + profile.t[2] + profile.t[4] + profile.t[6]); if (profile.check(tf, pf, vf, af, jMax, vMax, aMax)) { return true; @@ -347,20 +340,29 @@ bool Step2::time_up_vel(Profile& profile, double vMax, double aMax, double jMax) deriv[4] = 2./6 * polynom[4]; deriv[5] = 1./6 * polynom[5]; - auto dd_extremas = Roots::solveQuartMonic(4./5 * deriv[1], 3./5 * deriv[2], 2./5 * deriv[3], 1./5 * deriv[4]); - std::set> dd_tz_intervals; + std::array dderiv; + dderiv[0] = 1.0; + dderiv[1] = 4./5 * deriv[1]; + dderiv[2] = 3./5 * deriv[2]; + dderiv[3] = 2./5 * deriv[3]; + dderiv[4] = 1./5 * deriv[4]; + + auto dd_extremas = Roots::solveQuartMonic(dderiv); + std::set> dd_tz_intervals; double tz_min {0.0}; double tz_max = std::min(tf, (tf - a0/jMax) / 2); double dd_tz_current {tz_min}; for (double tz: dd_extremas) { - // std::cout << "tz: " << tz << std::endl; if (tz <= 0.0 || tz >= tz_max) { continue; } - // Check that polynom(lower) and polynom(upper) have different signs (should only happen at first and last boundary) + if (std::abs(Roots::polyEval(dderiv, tz)) > Roots::tolerance) { + tz -= Roots::polyEval(dderiv, tz) / Roots::polyEval(Roots::polyDeri(dderiv), tz); + } + if (Roots::polyEval(deriv, dd_tz_current) * Roots::polyEval(deriv, tz) < 0) { dd_tz_intervals.insert({dd_tz_current, tz}); } @@ -370,54 +372,57 @@ bool Step2::time_up_vel(Profile& profile, double vMax, double aMax, double jMax) dd_tz_intervals.insert({dd_tz_current, tz_max}); } - std::set> tz_intervals; + std::set roots; double tz_current {tz_min}; for (auto interval: dd_tz_intervals) { - const double lower = std::get<0>(interval); - const double upper = std::get<1>(interval); - const double tz = Roots::shrinkInterval(deriv, lower, upper); + const double tz = Roots::shrinkInterval(deriv, interval.first, interval.second); if (tz <= 0.0 || tz >= tz_max) { continue; } + + const double res = std::abs(Roots::polyEval(dderiv, tz)) * Roots::tolerance; const double p_val = Roots::polyEval(polynom, tz); - // Check that polynom(lower) and polynom(upper) have different signs (should only happen at first and last boundary) - // std::cout << "tzd: " << p_val << std::endl; - if (Roots::polyEval(polynom, tz_current) * p_val < 0) { - tz_intervals.insert({tz_current, tz}); - } else if (p_val < 1e-11) { - tz_intervals.insert({tz - 1e-12, tz + 1e-12}); - } + + if (std::abs(p_val) < res) { + roots.insert(tz); + } else if (Roots::polyEval(polynom, tz_current) * p_val < 0) { + roots.insert(Roots::shrinkInterval(polynom, tz_current, tz)); + } tz_current = tz; } if (Roots::polyEval(polynom, tz_current) * Roots::polyEval(polynom, tz_max) < 0) { - tz_intervals.insert({tz_current, tz_max}); + roots.insert(Roots::shrinkInterval(polynom, tz_current, tz_max)); } - for (auto interval: tz_intervals) { - // Use safe Newton method - const double lower = std::get<0>(interval); - const double upper = std::get<1>(interval); - double t = Roots::shrinkInterval(polynom, lower, upper); - - // std::cout << "t: " << t << std::endl; - + for (double t: roots) { // Single Newton step (regarding pd) { - const double h2 = -a0_a0 + af_af - 4*a0*jMax*t - 2*jMax*(jMax*t*t + v0 - vf); - const double orig = -pd + (-a0_p3 + af_p3 - 12*a0*jMax_jMax*t*(t - tf) + 3*a0_a0*jMax*(-2*t + tf) + 3*(af*(a0_a0 - af_af + 4*a0*jMax*t + 2*jMax*(jMax*t*t + v0 - vf))))/(6*jMax_jMax) + (jMax*t*t*(-t + tf) + tf*v0) + std::pow(h2,1.5)/(2*Sqrt(2)*jMax*Abs(jMax)); + const double h2 = af_af - a0_a0 - 4*a0*jMax*t - 2*jMax*(jMax*t*t - vd); + const double orig = -pd + (-a0_p3 + af_p3 - 12*a0*jMax_jMax*t*(t - tf) + 3*a0_a0*jMax*(-2*t + tf) + 3*(af*(-h2)))/(6*jMax_jMax) + (jMax*t*t*(-t + tf) + tf*v0) + std::pow(h2,1.5)/(2*Sqrt(2)*jMax*Abs(jMax)); const double deriv = -((a0 + jMax*t)*(3*jMax*Sqrt(2*h2)/Abs(jMax) - 4*af + 2*(a0 + 3*jMax*t - 2*jMax*tf)))/(2*jMax); t -= orig / deriv; } + + const double h1 = Sqrt((af_af - a0_a0)/2 - jMax*(2*a0*t + jMax*t*t - vd))/Abs(jMax); profile.t[0] = t; profile.t[1] = 0; profile.t[2] = profile.t[0] + a0/jMax; - profile.t[4] = Sqrt((af_af - a0_a0)/2 - jMax*(2*a0*t + jMax*t*t - vd))/Abs(jMax); + profile.t[3] = tf - 2*(t + h1) + ad/jMax; + profile.t[4] = h1; profile.t[5] = 0; profile.t[6] = profile.t[4] - af/jMax; - profile.t[3] = tf - (profile.t[0] + profile.t[2] + profile.t[4] + profile.t[6]); + + // std::cout << std::endl; + // std::cout << profile.t[0] << std::endl; + // std::cout << profile.t[1] << std::endl; + // std::cout << profile.t[2] << std::endl; + // std::cout << profile.t[3] << std::endl; + // std::cout << profile.t[4] << std::endl; + // std::cout << profile.t[5] << std::endl; + // std::cout << profile.t[6] << std::endl; if (profile.check(tf, pf, vf, af, jMax, vMax, aMax)) { return true; @@ -454,7 +459,7 @@ bool Step2::time_up_acc0_acc1(Profile& profile, double vMax, double aMax, double profile.t[2] = profile.t[0] + a0/jf; profile.t[3] = 0; profile.t[4] = profile.t[2]; - profile.t[5] = tf - (profile.t[0] + profile.t[1] + profile.t[2] + profile.t[3] + 2*profile.t[4] + af/jf); + profile.t[5] = tf - (profile.t[0] + profile.t[1] + profile.t[3] + 3*profile.t[2] + af/jf); profile.t[6] = profile.t[4] + af/jf; return profile.check(tf, pf, vf, af, jf, vMax, aMax, jMax); @@ -623,6 +628,16 @@ bool Step2::time_up_none(Profile& profile, double vMax, double aMax, double jMax continue; } + // Single Newton step (regarding pd) + { + const double h1 = a0_a0 + af_af + 2*af*jMax*t - 2*a0*(af + jMax*(t - tf)) + 2*jMax*(jMax*t*(t - tf) - vd); + const double h2 = (a0 - af + jMax*(-2*t + tf)); + const double orig = (-a0_p3 + af_p3 + 3*af_af*jMax*t + 3*af*jMax_jMax*Power(t,2) + 3*a0_a0*(af + jMax*t) - 3*a0*(af_af + 2*af*jMax*t + jMax_jMax*(Power(t,2) - Power(tf,2))) + 3*jMax_jMax*(-2*pd + jMax*t*(t - tf)*tf + 2*tf*v0))/(6.*jMax_jMax) - (jMax*std::pow(h1,1.5))/(2.*Sqrt(2)*Power(Abs(jMax),3)) + ((a0 - af - jMax*t)*(h1))/(2.*jMax_jMax); + const double deriv = (3*Sqrt(2)*Power(jMax,3)*h2*Sqrt(h1) - 2*jMax_jMax*(3*a0_a0 - 6*a0*af + 3*af_af + af*jMax*(8*t - 2*tf) + 4*a0*jMax*(-2*t + tf) + 2*jMax*(jMax*t*(3*t - 2*tf) - vd))*Abs(jMax) + 2*(a0 - af - jMax*tf)*h2*Power(Abs(jMax),3))/(4.*jMax*Power(Abs(jMax),3)); + + t -= orig / deriv; + } + const double h1 = Sqrt(2*(a0_a0 + af_af + 2*af*jMax*t - 2*a0*(af + jMax*(t - tf)) + 2*jMax*(jMax*t*(t - tf) - vd)))/Abs(jMax); // Solution 2 with aPlat diff --git a/test/otg-test.cpp b/test/otg-test.cpp index ff39af60..c9547df1 100644 --- a/test/otg-test.cpp +++ b/test/otg-test.cpp @@ -132,8 +132,8 @@ TEST_CASE("Ruckig") { constexpr bool full {true}; std::normal_distribution position_dist {0.0, 4.0}; - std::normal_distribution dynamic_dist {0.0, 0.6}; - std::uniform_real_distribution limit_dist {0.06, 12.0}; + std::normal_distribution dynamic_dist {0.0, 0.8}; + std::uniform_real_distribution limit_dist {0.05, 12.0}; SECTION("Known examples") { Ruckig<3> otg {0.005}; diff --git a/test/otg_plot.py b/test/otg_plot.py index 32e6b49c..db9b9256 100644 --- a/test/otg_plot.py +++ b/test/otg_plot.py @@ -76,7 +76,7 @@ def plot_trajectory(t_list, out_list): plt.xlabel('t') plt.savefig(Path(__file__).parent.parent / 'build' / 'otg_trajectory.png') - # plt.show() + plt.show() def print_input_for_mathematica(inp, dof, tf=None): @@ -97,22 +97,22 @@ def print_input_for_mathematica(inp, dof, tf=None): if __name__ == '__main__': inp = InputParameter() - inp.current_position = [3.5581310248, 0, 0.0] - inp.current_velocity = [-1.15961424805, 0.0, 0.0] - inp.current_acceleration = [0, -0.0, -0.0] - inp.target_position = [3.0870982243, 0.0, 0.0] - inp.target_velocity = [0, -0.0, -0.0] - inp.target_acceleration = [0, 0, 0] - inp.max_velocity = [8.5053447283, 3.17657, 7.51406] - inp.max_acceleration = [11.5220187169, 2.36737, 10.8257] - inp.max_jerk = [7.02808155416, 3.09066, 1.28268] - - # print_input_for_mathematica(inp, 0) + inp.current_position = [-2.0176247756, 0.0, 0.0] + inp.current_velocity = [-0.22271144785, 0.0, 0.0] + inp.current_acceleration = [1.41933174435, 0.0, 0.0] + inp.target_position = [6.474094467, 0.0, 0.0] + inp.target_velocity = [1.267076033443, 0.0, 0.0] + inp.target_acceleration = [0, 0.0, 0.0] + inp.max_velocity = [7.87691201185, 1.0, 1.0] + inp.max_acceleration = [2.00135800905, 1.0, 1.0] + inp.max_jerk = [0.0002, 1.0, 1.0] + + print_input_for_mathematica(inp, 0) # otg = Quintic(0.005) # otg = Smoothie(0.005) - # otg = Reflexxes(0.005) - otg = Ruckig(0.005) + # otg = Reflexxes(1000.025) + otg = Ruckig(1000.0) t_list, out_list = walk_through_trajectory(otg, inp)