From abac2303b240304de6671c21d4dc4110b108440b Mon Sep 17 00:00:00 2001 From: pantor Date: Mon, 21 Dec 2020 18:13:06 +0100 Subject: [PATCH] add license --- LICENSE | 50 +++ README.md | 44 ++- notebooks/ruckig-step2.nb | 812 ++++++++++++++------------------------ src/step2.cpp | 118 +++--- 4 files changed, 444 insertions(+), 580 deletions(-) create mode 100644 LICENSE diff --git a/LICENSE b/LICENSE new file mode 100644 index 00000000..8526e422 --- /dev/null +++ b/LICENSE @@ -0,0 +1,50 @@ +MIT License + +Copyright (c) 2021 Lars Berscheid + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. + + +--- + + +Copyright (c) 2009-2021 Lars Berscheid +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the FIRST nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY FIRST AND CONTRIBUTORS``AS IS'' AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/README.md b/README.md index ec20ee0f..5883bb86 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,43 @@ -# Ruckig +
+

Ruckig

+

+ Online Trajectory Generation. Real-time. Jerk-constrained. +

+
+

+ + CI + -Online Trajectory Generation. Real-time. Jerk-constrained. \ No newline at end of file + + Issues + + + + Releases + + + + LGPL + +

+ + +## Installation + +```bash +mkdir -p build +cd build +cmake -DBUILD_TYPE=Release .. +make +make install +``` + +## Development + +Frankx is written in C++17. It is currently tested against following versions + +- Eigen v3.3.9 +- Catch2 v2.13.3 (only for testing) +- Reflexxes v1.2.7 (only for testing) +- Pybind11 v2.6.0 (only for prototyping) diff --git a/notebooks/ruckig-step2.nb b/notebooks/ruckig-step2.nb index 99c061e6..4d350d5d 100644 --- a/notebooks/ruckig-step2.nb +++ b/notebooks/ruckig-step2.nb @@ -10,10 +10,10 @@ NotebookFileLineBreakTest NotebookFileLineBreakTest NotebookDataPosition[ 158, 7] -NotebookDataLength[ 66961, 1763] -NotebookOptionsPosition[ 63915, 1707] -NotebookOutlinePosition[ 64337, 1724] -CellTagsIndexPosition[ 64294, 1721] +NotebookDataLength[ 59800, 1547] +NotebookOptionsPosition[ 56755, 1491] +NotebookOutlinePosition[ 57177, 1508] +CellTagsIndexPosition[ 57134, 1505] WindowFrame->Normal*) (* Beginning of Notebook Content *) @@ -256,9 +256,10 @@ Cell[BoxData[{ 3.817206432896442*^9, 3.817267327497333*^9, 3.817273066602022*^9, { 3.817472122381835*^9, 3.817472123717684*^9}, {3.81747221413058*^9, 3.81747221438645*^9}, 3.8174915146800756`*^9, 3.817530512234641*^9, - 3.817539624223989*^9, 3.817539757155711*^9}, + 3.817539624223989*^9, 3.817539757155711*^9, 3.8175564232104387`*^9, + 3.817556749971588*^9}, CellLabel-> - "In[363]:=",ExpressionUUID->"9382707b-2876-460d-b0fb-ca70e05b1e9a"], + "In[179]:=",ExpressionUUID->"9382707b-2876-460d-b0fb-ca70e05b1e9a"], Cell[BoxData[ RowBox[{ @@ -325,36 +326,48 @@ Cell[BoxData[ RowBox[{ RowBox[{"a7", "\[Equal]", "af"}], "/.", RowBox[{"{", - RowBox[{"a0", "\[Rule]", "0"}], "}"}]}], ")"}], ",", + RowBox[{ + RowBox[{"a0", "\[Rule]", "0"}], ",", + RowBox[{"af", "\[Rule]", "0"}]}], "}"}]}], ")"}], ",", RowBox[{ RowBox[{"(", RowBox[{"v7", "/.", RowBox[{"{", - RowBox[{"a0", "\[Rule]", "0"}], "}"}]}], ")"}], "\[Equal]", + RowBox[{ + RowBox[{"a0", "\[Rule]", "0"}], ",", + RowBox[{"af", "\[Rule]", "0"}]}], "}"}]}], ")"}], "\[Equal]", "vf"}], ",", RowBox[{ RowBox[{"(", RowBox[{"p7", "/.", RowBox[{"{", - RowBox[{"a0", "\[Rule]", "0"}], "}"}]}], ")"}], "\[Equal]", + RowBox[{ + RowBox[{"a0", "\[Rule]", "0"}], ",", + RowBox[{"af", "\[Rule]", "0"}]}], "}"}]}], ")"}], "\[Equal]", "pf"}], ",", RowBox[{ RowBox[{"(", RowBox[{"a3", "/.", RowBox[{"{", - RowBox[{"a0", "\[Rule]", "0"}], "}"}]}], ")"}], "\[Equal]", + RowBox[{ + RowBox[{"a0", "\[Rule]", "0"}], ",", + RowBox[{"af", "\[Rule]", "0"}]}], "}"}]}], ")"}], "\[Equal]", "0"}], ",", RowBox[{ RowBox[{"(", RowBox[{"a1", "/.", RowBox[{"{", - RowBox[{"a0", "\[Rule]", "0"}], "}"}]}], ")"}], "\[Equal]", + RowBox[{ + RowBox[{"a0", "\[Rule]", "0"}], ",", + RowBox[{"af", "\[Rule]", "0"}]}], "}"}]}], ")"}], "\[Equal]", "aMax"}], ",", RowBox[{ RowBox[{"(", RowBox[{"a5", "/.", RowBox[{"{", - RowBox[{"a0", "\[Rule]", "0"}], "}"}]}], ")"}], "\[Equal]", + RowBox[{ + RowBox[{"a0", "\[Rule]", "0"}], ",", + RowBox[{"af", "\[Rule]", "0"}]}], "}"}]}], ")"}], "\[Equal]", RowBox[{"-", "aMax"}]}], ",", RowBox[{"t4", "\[Equal]", "0"}], ",", RowBox[{"tAll", "\[Equal]", "tf"}]}], "}"}], ",", @@ -366,7 +379,7 @@ Cell[BoxData[ RowBox[{ RowBox[{"{", RowBox[{ - RowBox[{"a7", "\[Equal]", "0"}], ",", + RowBox[{"a7", "\[Equal]", "af"}], ",", RowBox[{"v7", "\[Equal]", "vf"}], ",", RowBox[{"p7", "\[Equal]", "pf"}], ",", RowBox[{"a3", "\[Equal]", "0"}], ",", @@ -385,7 +398,7 @@ Cell[BoxData[ RowBox[{ RowBox[{"{", RowBox[{ - RowBox[{"a7", "\[Equal]", "0"}], ",", + RowBox[{"a7", "\[Equal]", "af"}], ",", RowBox[{"v7", "\[Equal]", "vf"}], ",", RowBox[{"p7", "\[Equal]", "pf"}], ",", RowBox[{"a3", "\[Equal]", "0"}], ",", @@ -586,22 +599,24 @@ Cell[BoxData[ 3.8174747809412537`*^9, 3.817474786461072*^9}, {3.817490686824304*^9, 3.8174906869494677`*^9}, {3.817491156287717*^9, 3.817491162863855*^9}, { 3.817491313811269*^9, 3.81749131393857*^9}, {3.817539498083053*^9, - 3.817539499801177*^9}, {3.817539754038406*^9, 3.817539754218478*^9}}, + 3.817539499801177*^9}, {3.817539754038406*^9, 3.817539754218478*^9}, { + 3.817556107819336*^9, 3.8175561104785957`*^9}, {3.817556816140629*^9, + 3.817556833804111*^9}}, CellLabel-> - "In[389]:=",ExpressionUUID->"93a49805-7b39-4e71-a5ce-78f2b77ae9c0"], + "In[206]:=",ExpressionUUID->"93a49805-7b39-4e71-a5ce-78f2b77ae9c0"], Cell["\<\ Information -- Case 1a: Acc0_Acc1_Vel (UDDU) Solution 1 -- Case 1b: Acc0_Acc1_Vel (UDUD) Solution 1 -- Case 2a: Acc0_Acc1 Solution 1 -- Case 2b: Acc0_Acc1 (a0=0) Solution ? -- Case 3a: Acc1_Vel (UDDU) -- Case 3b: Acc1_Vel (UDUD) -- Case 4: Acc0_Vel (UDDU), Root t5 -- Case 5: Vel, Root t1 -- Case 8a: None -- Case 8b: None Solution 1/2\ +- (1) Case 1a: Acc0_Acc1_Vel (UDDU) Solution 1 +- (2) Case 1b: Acc0_Acc1_Vel (UDUD) Solution 1 +- (3) Case 2a: Acc0_Acc1 Solution 1 +- (4) Case 2b: Acc0_Acc1 (a0=0, af=0) Solution 1 +- (5) Case 3a: Acc1_Vel (UDDU), Root t1 +- (6) Case 3b: Acc1_Vel (UDUD), Root t1 +- (7) Case 4: Acc0_Vel (UDDU), Root t5 +- (8) Case 5: Vel, Root t1 +- (9) Case 8a: None +- (10) Case 8b: None Solution 1/2\ \>", "Text", CellChangeTimes->{{3.817013988142602*^9, 3.8170139903294573`*^9}, { 3.817114182309226*^9, 3.817114185980912*^9}, {3.817114289998136*^9, @@ -611,7 +626,11 @@ Information 3.817474635933146*^9, 3.817474759233317*^9}, {3.817490334158193*^9, 3.81749048191954*^9}, {3.817491196013754*^9, 3.817491197517359*^9}, { 3.817530590294873*^9, 3.817530591542695*^9}, {3.817539682775482*^9, - 3.817539682839389*^9}},ExpressionUUID->"0616ae41-3a02-473a-878f-\ + 3.817539682839389*^9}, {3.8175561219147263`*^9, 3.81755614101873*^9}, { + 3.817556171730296*^9, 3.8175561835139*^9}, {3.817556764334896*^9, + 3.817556765630618*^9}, {3.817556840496716*^9, 3.817556851456852*^9}, { + 3.817556975301791*^9, + 3.817556975327032*^9}},ExpressionUUID->"0616ae41-3a02-473a-878f-\ 96412e2fc8c5"], Cell["\<\ @@ -630,124 +649,151 @@ Cell[BoxData[{ RowBox[{"Simplify", "[", RowBox[{"resultT", "[", RowBox[{"[", - RowBox[{"4", ",", "1", ",", "1", ",", "2"}], "]"}], "]"}], + RowBox[{"3", ",", "1", ",", "8", ",", "2"}], "]"}], "]"}], "]"}]}], "\[IndentingNewLine]", RowBox[{ RowBox[{"ToString", "[", RowBox[{"tmp", ",", "CForm"}], "]"}], ";"}], "\[IndentingNewLine]", RowBox[{"CopyToClipboard", "[", "%", "]"}]}], "Input", CellChangeTimes->CompressedData[" -1:eJxTTMoPSmViYGAQA2IQfeTRnqfxBW8cpyy99QZEC/zI9EoA0n680sEg2slA +1:eJxTTMoPSmViYGCQBGIQfeTRnqfxBW8cpyy99QZEC/zI9EoA0n680sEg2slA w2sikD5X+TAARBv5dYeB6GaHXjDNe/lfEog+8PpOBojmsnlTD6LnzOBqA9FL sn74TgPSRbWioSDa7kje+hlAOj3xzA4QLbbw4yEQveryZzD9hrv3BIiuSOkD 0zmKv8+A+X1/wPS370ziu4D0vKssciBajStLzqHwjWNS/1dlEC1zyY7TE0S3 OYDp+93HRUB0bNBUURAdkzZbGUQbMbGqgOii97XaIHresXowvSS71RJEn7kq YQOip2WYeYPoH30nfED0Ps+p3xOAdD/Pv38gepdYPkcikD53lYUPRB85oCkB -on9YeoFpAJc0sPU= +on9YeoHpAqnPRZlAWqg1rQxE7+DwaQLR/V1+YPrAUsl2EL2s0XgKiAYA89fH +7g== "], CellLabel-> - "In[390]:=",ExpressionUUID->"f08feb2e-6079-473b-b866-92670dea0a56"], + "In[268]:=",ExpressionUUID->"f08feb2e-6079-473b-b866-92670dea0a56"], Cell[BoxData[ - RowBox[{ + RowBox[{"-", RowBox[{"(", - RowBox[{"2", " ", "aMax", " ", + RowBox[{ RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "6"}], " ", - SuperscriptBox["aMax", "3"], " ", "tf"}], "-", - RowBox[{"3", " ", - SuperscriptBox["af", "2"], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"aMax", " ", "tf"}], "+", "v0", "-", "vf"}], ")"}]}], "-", - RowBox[{"6", " ", "af", " ", "aMax", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"aMax", " ", "tf"}], "+", "v0", "-", "vf"}], ")"}]}], "+", - RowBox[{ - SqrtBox["6"], " ", + RowBox[{"-", "12"}], " ", + SuperscriptBox["a0", "2"], " ", "aMax", " ", "tf"}], "-", + RowBox[{"12", " ", + SuperscriptBox["af", "2"], " ", "aMax", " ", "tf"}], "+", + RowBox[{"24", " ", "a0", " ", + SuperscriptBox["aMax", "2"], " ", "tf"}], "-", + RowBox[{"24", " ", "af", " ", + SuperscriptBox["aMax", "2"], " ", "tf"}], "-", + RowBox[{"24", " ", + SuperscriptBox["aMax", "3"], " ", "tf"}], "+", + RowBox[{"12", " ", + SuperscriptBox["a0", "2"], " ", "v0"}], "-", + RowBox[{"12", " ", + SuperscriptBox["af", "2"], " ", "v0"}], "-", + RowBox[{"24", " ", "a0", " ", "aMax", " ", "v0"}], "-", + RowBox[{"24", " ", "af", " ", "aMax", " ", "v0"}], "-", + RowBox[{"12", " ", + SuperscriptBox["a0", "2"], " ", "vf"}], "+", + RowBox[{"12", " ", + SuperscriptBox["af", "2"], " ", "vf"}], "+", + RowBox[{"24", " ", "a0", " ", "aMax", " ", "vf"}], "+", + RowBox[{"24", " ", "af", " ", "aMax", " ", "vf"}], "-", + RowBox[{"4", " ", RowBox[{"\[Sqrt]", RowBox[{"(", - RowBox[{"aMax", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"6", " ", - SuperscriptBox["aMax", "5"], " ", - SuperscriptBox["tf", "2"]}], "+", - RowBox[{"3", " ", - SuperscriptBox["af", "4"], " ", - RowBox[{"(", + RowBox[{ + RowBox[{"9", " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", + SuperscriptBox["aMax", "3"], " ", "tf"}], "+", RowBox[{ - RowBox[{"2", " ", "p0"}], "-", - RowBox[{"2", " ", "pf"}], "+", - RowBox[{"aMax", " ", - SuperscriptBox["tf", "2"]}], "+", - RowBox[{"2", " ", "tf", " ", "v0"}]}], ")"}]}], "+", - RowBox[{"6", " ", - SuperscriptBox["af", "2"], " ", "aMax", " ", - RowBox[{"(", + SuperscriptBox["af", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"aMax", " ", "tf"}], "+", "v0", "-", "vf"}], ")"}]}], + "+", + RowBox[{"2", " ", "af", " ", "aMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"aMax", " ", "tf"}], "+", "v0", "-", "vf"}], ")"}]}], + "+", RowBox[{ + SuperscriptBox["a0", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"aMax", " ", "tf"}], "-", "v0", "+", "vf"}], ")"}]}], + "-", + RowBox[{"2", " ", "a0", " ", "aMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"aMax", " ", "tf"}], "-", "v0", "+", "vf"}], + ")"}]}]}], ")"}], "2"]}], "+", + RowBox[{"3", " ", + RowBox[{"(", + RowBox[{"a0", "-", "af"}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"3", " ", + SuperscriptBox["a0", "3"]}], "-", + RowBox[{"3", " ", + SuperscriptBox["af", "3"]}], "+", + RowBox[{ + SuperscriptBox["a0", "2"], " ", + RowBox[{"(", RowBox[{ - SuperscriptBox["aMax", "2"], " ", - SuperscriptBox["tf", "2"]}], "+", - RowBox[{"2", " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{"v0", "-", "vf"}], ")"}], "2"]}], "+", - RowBox[{"aMax", " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "4"}], " ", "p0"}], "+", - RowBox[{"4", " ", "pf"}], "+", - RowBox[{"tf", " ", "v0"}], "-", - RowBox[{"5", " ", "tf", " ", "vf"}]}], ")"}]}]}], ")"}]}], - "+", - RowBox[{"4", " ", - SuperscriptBox["af", "3"], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"2", " ", - SuperscriptBox["aMax", "2"], " ", - SuperscriptBox["tf", "2"]}], "+", - SuperscriptBox[ - RowBox[{"(", - RowBox[{"v0", "-", "vf"}], ")"}], "2"], "+", - RowBox[{"2", " ", "aMax", " ", - RowBox[{"(", - RowBox[{"p0", "-", "pf", "+", - RowBox[{"2", " ", "tf", " ", "v0"}], "-", - RowBox[{"tf", " ", "vf"}]}], ")"}]}]}], ")"}]}], "-", - RowBox[{"12", " ", "af", " ", - SuperscriptBox["aMax", "2"], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"-", - SuperscriptBox[ + RowBox[{"3", " ", "af"}], "-", + RowBox[{"4", " ", "aMax"}]}], ")"}]}], "-", + RowBox[{"4", " ", + SuperscriptBox["af", "2"], " ", "aMax"}], "+", + RowBox[{"12", " ", "af", " ", + SuperscriptBox["aMax", "2"]}], "+", + RowBox[{"24", " ", + SuperscriptBox["aMax", "3"]}], "-", + RowBox[{"a0", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"3", " ", + SuperscriptBox["af", "2"]}], "+", + RowBox[{"16", " ", "af", " ", "aMax"}], "+", + RowBox[{"12", " ", + SuperscriptBox["aMax", "2"]}]}], ")"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + SuperscriptBox["aMax", "2"], " ", + SuperscriptBox["tf", "2"]}], "-", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"], "+", + RowBox[{"2", " ", "aMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "p0"}], "-", + RowBox[{"2", " ", "pf"}], "+", + RowBox[{"tf", " ", RowBox[{"(", - RowBox[{"v0", "-", "vf"}], ")"}], "2"]}], "+", - RowBox[{"aMax", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"4", " ", "p0"}], "-", - RowBox[{"4", " ", "pf"}], "+", - RowBox[{"tf", " ", "v0"}], "+", - RowBox[{"3", " ", "tf", " ", "vf"}]}], ")"}]}]}], ")"}]}]}], - ")"}]}], ")"}]}]}]}], ")"}]}], ")"}], "/", - RowBox[{"(", - RowBox[{"af", " ", + RowBox[{"v0", "+", "vf"}], ")"}]}]}], ")"}]}]}], ")"}]}]}], + ")"}]}]}]}], ")"}], "/", RowBox[{"(", - RowBox[{ - RowBox[{"3", " ", - SuperscriptBox["af", "3"]}], "+", - RowBox[{"4", " ", - SuperscriptBox["af", "2"], " ", "aMax"}], "-", - RowBox[{"12", " ", "af", " ", - SuperscriptBox["aMax", "2"]}], "-", - RowBox[{"24", " ", - SuperscriptBox["aMax", "3"]}]}], ")"}]}], ")"}]}]], "Output", + RowBox[{"24", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + SuperscriptBox["aMax", "2"], " ", + SuperscriptBox["tf", "2"]}], "-", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"], "+", + RowBox[{"2", " ", "aMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "p0"}], "-", + RowBox[{"2", " ", "pf"}], "+", + RowBox[{"tf", " ", + RowBox[{"(", + RowBox[{"v0", "+", "vf"}], ")"}]}]}], ")"}]}]}], ")"}]}], + ")"}]}], ")"}]}]], "Output", CellChangeTimes->{{3.817013038100574*^9, 3.8170130952655783`*^9}, { 3.817013843881184*^9, 3.817013913172606*^9}, 3.817013966856229*^9, { 3.8171141944209747`*^9, 3.817114247671588*^9}, 3.8171142928777733`*^9, { @@ -760,9 +806,11 @@ Cell[BoxData[ 3.817490606780237*^9}, 3.8174907229493113`*^9, 3.8174907869144897`*^9, { 3.817490894583593*^9, 3.817490915801375*^9}, {3.817491035943132*^9, 3.817491041107473*^9}, {3.8175395210316668`*^9, 3.817539576386404*^9}, { - 3.817539653966858*^9, 3.8175396965766487`*^9}, 3.817539782239129*^9}, + 3.817539653966858*^9, 3.8175396965766487`*^9}, 3.817539782239129*^9, + 3.817556857265253*^9, {3.817556888055502*^9, 3.817556915762059*^9}, { + 3.817557012891211*^9, 3.817557155732799*^9}}, CellLabel-> - "Out[390]=",ExpressionUUID->"fb0d3b69-6fc5-442b-8456-51f7f65e0c32"] + "Out[268]=",ExpressionUUID->"faa343c3-02f2-4fc4-b2b0-2208fa60cf35"] }, Open ]], Cell["Print Roots", "Text", @@ -780,7 +828,7 @@ Cell[BoxData[{ RowBox[{"tmpResult", "=", RowBox[{"resultT", "[", RowBox[{"[", - RowBox[{"8", ",", "1"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"6", ",", "1"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ RowBox[{"rootFunction", "=", RowBox[{"tmpResult", "[", @@ -797,7 +845,7 @@ Cell[BoxData[{ RowBox[{"rootFunction", "[", RowBox[{"[", "1", "]"}], "]"}], "[", "x", "]"}], ",", "x"}], "]"}], "[", - RowBox[{"[", "5", "]"}], "]"}], + RowBox[{"[", "1", "]"}], "]"}], RowBox[{"Last", "[", RowBox[{"CoefficientList", "[", RowBox[{ @@ -812,31 +860,74 @@ Cell[BoxData[{ CellChangeTimes->{{3.8174912269910707`*^9, 3.8174912369981537`*^9}, { 3.817491274208914*^9, 3.817491290374673*^9}, {3.8174913367940817`*^9, 3.817491349584128*^9}, {3.817491538900215*^9, 3.817491568052608*^9}, { - 3.8175305970172663`*^9, 3.817530673061006*^9}}, + 3.8175305970172663`*^9, 3.817530673061006*^9}, {3.817556188570257*^9, + 3.8175562372327757`*^9}, {3.8175565493162394`*^9, 3.817556555010408*^9}, { + 3.817556590540579*^9, 3.8175566069951468`*^9}}, CellLabel-> - "In[252]:=",ExpressionUUID->"217d7885-1018-483e-8952-08082feb5695"], + "In[145]:=",ExpressionUUID->"217d7885-1018-483e-8952-08082feb5695"], Cell[BoxData[ - FractionBox[ - RowBox[{ - RowBox[{"15", " ", - SuperscriptBox["a0", "2"]}], "+", - RowBox[{"16", " ", "a0", " ", "jMax", " ", "tf"}], "-", - RowBox[{"2", " ", - SuperscriptBox["jMax", "2"], " ", - SuperscriptBox["tf", "2"]}], "+", - RowBox[{"6", " ", "jMax", " ", "v0"}], "-", - RowBox[{"6", " ", "jMax", " ", "vf"}]}], - RowBox[{ - RowBox[{"4", " ", "a0", " ", "jMax"}], "+", - RowBox[{"4", " ", - SuperscriptBox["jMax", "2"], " ", "tf"}]}]]], "Output", + RowBox[{ + FractionBox["1", + RowBox[{"12", " ", + SuperscriptBox["jMax", "4"]}]], + RowBox[{"(", + RowBox[{ + RowBox[{"3", " ", + SuperscriptBox["a0", "4"]}], "+", + RowBox[{"3", " ", + SuperscriptBox["af", "4"]}], "-", + RowBox[{"4", " ", + SuperscriptBox["a0", "3"], " ", "aMax"}], "-", + RowBox[{"8", " ", + SuperscriptBox["af", "3"], " ", "aMax"}], "+", + RowBox[{"24", " ", "af", " ", "aMax", " ", "jMax", " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}]}], "-", + RowBox[{"6", " ", + SuperscriptBox["a0", "2"], " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["af", "2"], "-", + RowBox[{"2", " ", "af", " ", "aMax"}], "+", + SuperscriptBox["aMax", "2"], "-", + RowBox[{"2", " ", "aMax", " ", "jMax", " ", "tf"}], "-", + RowBox[{"2", " ", "jMax", " ", "v0"}], "+", + RowBox[{"2", " ", "jMax", " ", "vf"}]}], ")"}]}], "+", + RowBox[{"12", " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "aMax", " ", "jMax", " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf", "+", + RowBox[{"tf", " ", "v0"}]}], ")"}]}], "+", + RowBox[{"jMax", " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"]}], "+", + RowBox[{ + SuperscriptBox["aMax", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "v0"}], "+", "vf"}], ")"}]}]}], ")"}]}], "+", + RowBox[{"6", " ", + SuperscriptBox["af", "2"], " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["aMax", "2"], "+", + RowBox[{"2", " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "v0"}], "+", "vf"}], ")"}]}]}], ")"}]}]}], + ")"}]}]], "Output", CellChangeTimes->{{3.817491232262742*^9, 3.81749123748594*^9}, { 3.817491275061132*^9, 3.8174912908159*^9}, {3.817491337280683*^9, 3.8174913499892473`*^9}, {3.817491546457255*^9, 3.8174915684457703`*^9}, { - 3.8175305994860983`*^9, 3.817530673444901*^9}, 3.817534920459261*^9}, + 3.8175305994860983`*^9, 3.817530673444901*^9}, 3.817534920459261*^9, { + 3.8175561893975782`*^9, 3.817556237683996*^9}, {3.8175565534030046`*^9, + 3.81755660742447*^9}}, CellLabel-> - "Out[255]=",ExpressionUUID->"c8c1ef8b-557d-4acc-8854-09d94a878b1a"] + "Out[148]=",ExpressionUUID->"2e4b2150-6f82-4c7d-8607-3649724667c6"] }, Open ]], Cell[CellGroupData[{ @@ -861,348 +952,29 @@ Cell[BoxData[{ 3.817491588286084*^9, 3.817491628985776*^9}, {3.8175306940387907`*^9, 3.8175307010854073`*^9}, {3.817530733671631*^9, 3.817530733709938*^9}, { 3.8175349306462812`*^9, 3.8175349353248568`*^9}, {3.817534969175642*^9, - 3.817534981662369*^9}}, + 3.817534981662369*^9}, {3.8175562930510178`*^9, 3.817556322147118*^9}, { + 3.817556643892717*^9, 3.817556671396356*^9}}, CellLabel-> - "In[282]:=",ExpressionUUID->"58c30e25-83c4-4d05-ad21-e88fcb7f4181"], + "In[175]:=",ExpressionUUID->"58c30e25-83c4-4d05-ad21-e88fcb7f4181"], Cell[BoxData[ - RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{"12", " ", - SuperscriptBox["a0", "7"]}], "+", - RowBox[{ - SuperscriptBox["a0", "6"], " ", "jMax", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"101", " ", "t"}], "+", - RowBox[{"19", " ", "tf"}]}], ")"}]}], "+", - RowBox[{"6", " ", - SuperscriptBox["a0", "5"], " ", "jMax", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"45", " ", "jMax", " ", - SuperscriptBox["t", "2"]}], "+", - RowBox[{"22", " ", "jMax", " ", "t", " ", "tf"}], "-", - RowBox[{"5", " ", "jMax", " ", - SuperscriptBox["tf", "2"]}], "+", - RowBox[{"6", " ", "v0"}], "-", - RowBox[{"6", " ", "vf"}]}], ")"}]}], "+", - RowBox[{"6", " ", - SuperscriptBox["a0", "4"], " ", - SuperscriptBox["jMax", "2"], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"6", " ", "p0"}], "-", - RowBox[{"6", " ", "pf"}], "+", - RowBox[{"jMax", " ", "t", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"41", " ", - SuperscriptBox["t", "2"]}], "+", - RowBox[{"52", " ", "t", " ", "tf"}], "-", - RowBox[{"23", " ", - SuperscriptBox["tf", "2"]}]}], ")"}]}], "-", - RowBox[{"7", " ", "tf", " ", "v0"}], "+", - RowBox[{"11", " ", "t", " ", - RowBox[{"(", - RowBox[{"v0", "-", "vf"}], ")"}]}], "+", - RowBox[{"13", " ", "tf", " ", "vf"}]}], ")"}]}], "+", - RowBox[{"12", " ", - SuperscriptBox["a0", "3"], " ", - SuperscriptBox["jMax", "2"], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - SuperscriptBox["jMax", "2"], " ", - SuperscriptBox["t", "2"], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"6", " ", - SuperscriptBox["t", "2"]}], "+", - RowBox[{"22", " ", "t", " ", "tf"}], "-", - RowBox[{"11", " ", - SuperscriptBox["tf", "2"]}]}], ")"}]}], "-", - RowBox[{"8", " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{"v0", "-", "vf"}], ")"}], "2"]}], "+", - RowBox[{"jMax", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"16", " ", "p0", " ", "t"}], "-", - RowBox[{"16", " ", "pf", " ", "t"}], "-", - RowBox[{"4", " ", "p0", " ", "tf"}], "+", - RowBox[{"4", " ", "pf", " ", "tf"}], "-", - RowBox[{"25", " ", - SuperscriptBox["t", "2"], " ", "v0"}], "-", - RowBox[{"26", " ", "t", " ", "tf", " ", "v0"}], "+", - RowBox[{ - SuperscriptBox["tf", "2"], " ", "v0"}], "+", - RowBox[{"25", " ", - SuperscriptBox["t", "2"], " ", "vf"}], "+", - RowBox[{"42", " ", "t", " ", "tf", " ", "vf"}], "-", - RowBox[{"5", " ", - SuperscriptBox["tf", "2"], " ", "vf"}]}], ")"}]}]}], ")"}]}], "+", - - RowBox[{"144", " ", "a0", " ", - SuperscriptBox["jMax", "3"], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"-", - SuperscriptBox[ - RowBox[{"(", - RowBox[{"v0", "-", "vf"}], ")"}], "3"]}], "+", - RowBox[{"jMax", " ", - RowBox[{"(", - RowBox[{"v0", "-", "vf"}], ")"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"p0", " ", - RowBox[{"(", - RowBox[{"t", "+", "tf"}], ")"}]}], "-", - RowBox[{"pf", " ", - RowBox[{"(", - RowBox[{"t", "+", "tf"}], ")"}]}], "-", - RowBox[{"5", " ", - SuperscriptBox["t", "2"], " ", "v0"}], "+", - RowBox[{ - SuperscriptBox["tf", "2"], " ", "v0"}], "+", - RowBox[{"5", " ", - SuperscriptBox["t", "2"], " ", "vf"}], "+", - RowBox[{"t", " ", "tf", " ", "vf"}]}], ")"}]}], "+", - RowBox[{ - SuperscriptBox["jMax", "2"], " ", "t", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"p0", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["t", "2"], "+", - RowBox[{"2", " ", "t", " ", "tf"}], "-", - SuperscriptBox["tf", "2"]}], ")"}]}], "+", - RowBox[{"pf", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"-", - SuperscriptBox["t", "2"]}], "-", - RowBox[{"2", " ", "t", " ", "tf"}], "+", - SuperscriptBox["tf", "2"]}], ")"}]}], "-", - RowBox[{"t", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["t", "2"], "+", - RowBox[{"4", " ", "t", " ", "tf"}], "-", - RowBox[{"2", " ", - SuperscriptBox["tf", "2"]}]}], ")"}], " ", "v0"}], "+", - RowBox[{ - RowBox[{"(", - RowBox[{ - SuperscriptBox["t", "3"], "+", - RowBox[{"5", " ", - SuperscriptBox["t", "2"], " ", "tf"}], "-", - SuperscriptBox["tf", "3"]}], ")"}], " ", "vf"}]}], ")"}]}]}], - ")"}]}], "+", - RowBox[{"36", " ", - SuperscriptBox["a0", "2"], " ", - SuperscriptBox["jMax", "3"], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - SuperscriptBox["jMax", "2"], " ", - SuperscriptBox["t", "3"], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"2", " ", "t"}], "-", "tf"}], ")"}], " ", "tf"}], "-", - RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{"19", " ", "t"}], "+", "tf"}], ")"}], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{"v0", "-", "vf"}], ")"}], "2"]}], "-", - RowBox[{"jMax", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"pf", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"9", " ", - SuperscriptBox["t", "2"]}], "+", - RowBox[{"2", " ", "t", " ", "tf"}], "-", - SuperscriptBox["tf", "2"]}], ")"}]}], "+", - RowBox[{"p0", " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "9"}], " ", - SuperscriptBox["t", "2"]}], "-", - RowBox[{"2", " ", "t", " ", "tf"}], "+", - SuperscriptBox["tf", "2"]}], ")"}]}], "+", - RowBox[{"4", " ", "t", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"3", " ", "t"}], "-", "tf"}], ")"}], " ", - RowBox[{"(", - RowBox[{"t", "+", - RowBox[{"2", " ", "tf"}]}], ")"}], " ", "v0"}], "+", - RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "12"}], " ", - SuperscriptBox["t", "3"]}], "-", - RowBox[{"29", " ", - SuperscriptBox["t", "2"], " ", "tf"}], "+", - RowBox[{"6", " ", "t", " ", - SuperscriptBox["tf", "2"]}], "+", - SuperscriptBox["tf", "3"]}], ")"}], " ", "vf"}]}], ")"}]}]}], - ")"}]}], "+", - RowBox[{"72", " ", - SuperscriptBox["jMax", "4"], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - SuperscriptBox["jMax", "2"], " ", - SuperscriptBox["t", "2"], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"2", " ", "t"}], "-", "tf"}], ")"}], " ", "tf", " ", - RowBox[{"(", - RowBox[{"p0", "-", "pf", "-", - RowBox[{"t", " ", "v0"}], "+", - RowBox[{ - RowBox[{"(", - RowBox[{"t", "+", "tf"}], ")"}], " ", "vf"}]}], ")"}]}], "+", - RowBox[{ - SuperscriptBox[ - RowBox[{"(", - RowBox[{"v0", "-", "vf"}], ")"}], "2"], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"2", " ", "p0"}], "-", - RowBox[{"2", " ", "pf"}], "-", - RowBox[{"t", " ", "v0"}], "+", - RowBox[{"tf", " ", "v0"}], "+", - RowBox[{ - RowBox[{"(", - RowBox[{"t", "+", "tf"}], ")"}], " ", "vf"}]}], ")"}]}], "+", - RowBox[{"jMax", " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - SuperscriptBox["p0", "2"], " ", - RowBox[{"(", - RowBox[{"t", "-", "tf"}], ")"}]}], "+", - RowBox[{ - SuperscriptBox["pf", "2"], " ", - RowBox[{"(", - RowBox[{"t", "-", "tf"}], ")"}]}], "+", - RowBox[{"pf", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"-", - SuperscriptBox["t", "2"]}], "-", - RowBox[{"2", " ", "t", " ", "tf"}], "+", - SuperscriptBox["tf", "2"]}], ")"}], " ", "v0"}], "+", - RowBox[{"t", " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "3"}], " ", - SuperscriptBox["t", "2"]}], "+", - SuperscriptBox["tf", "2"]}], ")"}], " ", - SuperscriptBox["v0", "2"]}], "+", - RowBox[{"pf", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["t", "2"], "+", - SuperscriptBox["tf", "2"]}], ")"}], " ", "vf"}], "+", - RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{"6", " ", - SuperscriptBox["t", "3"]}], "+", - RowBox[{ - SuperscriptBox["t", "2"], " ", "tf"}], "-", - SuperscriptBox["tf", "3"]}], ")"}], " ", "v0", " ", "vf"}], "-", - - RowBox[{ - SuperscriptBox["t", "2"], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"3", " ", "t"}], "+", "tf"}], ")"}], " ", - SuperscriptBox["vf", "2"]}], "-", - RowBox[{"p0", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"2", " ", "pf", " ", - RowBox[{"(", - RowBox[{"t", "-", "tf"}], ")"}]}], "-", - RowBox[{"2", " ", "t", " ", "tf", " ", "v0"}], "+", - RowBox[{ - SuperscriptBox["t", "2"], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"-", "v0"}], "+", "vf"}], ")"}]}], "+", - RowBox[{ - SuperscriptBox["tf", "2"], " ", - RowBox[{"(", - RowBox[{"v0", "+", "vf"}], ")"}]}]}], ")"}]}]}], ")"}]}]}], - ")"}]}]}], ")"}], "/", - RowBox[{"(", - RowBox[{"jMax", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"-", - SuperscriptBox["a0", "6"]}], "+", - RowBox[{"6", " ", - SuperscriptBox["a0", "4"], " ", "jMax", " ", - RowBox[{"(", - RowBox[{"v0", "-", "vf"}], ")"}]}], "-", - RowBox[{"36", " ", - SuperscriptBox["a0", "2"], " ", - SuperscriptBox["jMax", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{"v0", "-", "vf"}], ")"}], "2"]}], "+", - RowBox[{"48", " ", - SuperscriptBox["a0", "3"], " ", - SuperscriptBox["jMax", "2"], " ", - RowBox[{"(", - RowBox[{"p0", "-", "pf", "+", - RowBox[{"tf", " ", "vf"}]}], ")"}]}], "-", - RowBox[{"144", " ", "a0", " ", - SuperscriptBox["jMax", "3"], " ", - RowBox[{"(", - RowBox[{"v0", "-", "vf"}], ")"}], " ", - RowBox[{"(", - RowBox[{"p0", "-", "pf", "+", - RowBox[{"tf", " ", "vf"}]}], ")"}]}], "+", - RowBox[{"72", " ", - SuperscriptBox["jMax", "3"], " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox[ - RowBox[{"(", - RowBox[{"v0", "-", "vf"}], ")"}], "3"], "+", - RowBox[{"jMax", " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{"p0", "-", "pf", "+", - RowBox[{"tf", " ", "vf"}]}], ")"}], "2"]}]}], ")"}]}]}], ")"}]}], - ")"}]}]], "Output", + FractionBox[ + RowBox[{ + RowBox[{"-", "af"}], "+", "aMax"}], "jMax"]], "Output", CellChangeTimes->{{3.817491375709132*^9, 3.817491418809086*^9}, { 3.817491592470599*^9, 3.817491629469158*^9}, {3.817530694487358*^9, 3.817530706354188*^9}, 3.817530737872798*^9, {3.817534931188129*^9, - 3.817534940127095*^9}, {3.8175349740190363`*^9, 3.817534982082616*^9}}, + 3.817534940127095*^9}, {3.8175349740190363`*^9, 3.817534982082616*^9}, { + 3.817556293641871*^9, 3.817556322632617*^9}, {3.817556644430108*^9, + 3.8175566719195757`*^9}}, CellLabel-> - "Out[283]=",ExpressionUUID->"480d0f52-423a-4af8-96f6-25e0827fcb2a"] + "Out[176]=",ExpressionUUID->"333c90a7-66b2-4ebd-90da-2deda377e542"] }, Open ]], -Cell["Case 9 - a3 != 0", "Text", - CellChangeTimes->{{3.817199297723555*^9, - 3.8171993034485407`*^9}},ExpressionUUID->"5c00d19c-efbb-475f-a4a4-\ +Cell["With acceleration Plateau -> a3 != 0", "Text", + CellChangeTimes->{{3.817199297723555*^9, 3.8171993034485407`*^9}, { + 3.817558314990168*^9, + 3.817558320332492*^9}},ExpressionUUID->"5c00d19c-efbb-475f-a4a4-\ adc1877059e7"], Cell[BoxData[ @@ -1333,29 +1105,41 @@ Cell[BoxData[ 3.817269550913919*^9, 3.817269582495365*^9}, {3.8172696597792683`*^9, 3.817269673093995*^9}}, CellLabel-> - "In[717]:=",ExpressionUUID->"013ba19d-162c-4e14-9e6b-43bcf125ceef"], + "In[271]:=",ExpressionUUID->"013ba19d-162c-4e14-9e6b-43bcf125ceef"], Cell["\<\ -Case1 -> 1 (UDDU) / 2 (UDUD), Case2 -> 1 (UDDU) / 2 (UDUD), Case 3 -> 1 \ -(UDDU), Case 4 -> 2 (UDDU), Case 5-> 1(UDUD), Case 6-> 1 (UDUD)\ +Information +- (1) None: Solution 1 (UDDU) / 2 (UDUD), Root t1 +- (2) None: Solution 1 (UDDU) / 2 (UDUD), Root t3 +- (3) Acc0: Solution 1 (UDDU) +- (4) Acc1: Solution 2 (UDDU) +- (5) Acc0: Solution ? (UDUD) +- (6) Acc1: Solution 1 (UDUD) + +Print Cases +[[Case, Solution, Time Number, 2]]\ \>", "Text", CellChangeTimes->{{3.8172658312535543`*^9, 3.817265834325848*^9}, { 3.817266381316019*^9, 3.817266387260919*^9}, {3.817266844193924*^9, 3.817266844265978*^9}, {3.817267941680891*^9, 3.817267951096839*^9}, { 3.817268271292224*^9, 3.817268280837641*^9}, {3.817269566668298*^9, - 3.8172696171423397`*^9}},ExpressionUUID->"ccf44efd-e09a-4bbb-a696-\ + 3.8172696171423397`*^9}, {3.817558152473523*^9, 3.817558304739996*^9}, { + 3.817558375206586*^9, 3.8175583806143312`*^9}, {3.8175584286412487`*^9, + 3.817558429086872*^9}},ExpressionUUID->"ccf44efd-e09a-4bbb-a696-\ b4c90f7a95eb"], Cell[CellGroupData[{ Cell[BoxData[{ - RowBox[{"ToString", "[", - RowBox[{ - RowBox[{"Simplify", "[", - RowBox[{"resultTa", "[", - RowBox[{"[", - RowBox[{"6", ",", "1", ",", "7", ",", "2"}], "]"}], "]"}], "]"}], ",", - "CForm"}], "]"}], "\[IndentingNewLine]", + RowBox[{"tmp", "=", + RowBox[{"Simplify", "[", + RowBox[{"resultTa", "[", + RowBox[{"[", + RowBox[{"6", ",", "1", ",", "7", ",", "2"}], "]"}], "]"}], + "]"}]}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"ToString", "[", + RowBox[{"tmp", ",", "CForm"}], "]"}], ";"}], "\[IndentingNewLine]", RowBox[{"CopyToClipboard", "[", "%", "]"}]}], "Input", CellChangeTimes->{{3.817265499752473*^9, 3.817265502986458*^9}, { 3.817265639391837*^9, 3.817265642449834*^9}, {3.81726571679879*^9, @@ -1364,9 +1148,9 @@ Cell[BoxData[{ 3.8172663321244926`*^9}, {3.817266412616894*^9, 3.8172664466999893`*^9}, { 3.8172668459738197`*^9, 3.817266895760104*^9}, {3.817268767761549*^9, 3.817268771935257*^9}, {3.8172688263122473`*^9, 3.817268880737111*^9}, { - 3.817269690126698*^9, 3.817269740905311*^9}}, - CellLabel-> - "In[689]:=",ExpressionUUID->"b4fd3303-7c30-4af8-94b1-4505577eaf40"], + 3.817269690126698*^9, 3.817269740905311*^9}, {3.817558355798354*^9, + 3.817558365794586*^9}},ExpressionUUID->"b4fd3303-7c30-4af8-94b1-\ +4505577eaf40"], Cell[BoxData["\<\"aMax/jMax\"\>"], "Output", CellChangeTimes->{ @@ -1706,7 +1490,7 @@ Cell[BoxData[ }, Open ]] }, WindowSize->{904, 920}, -WindowMargins->{{62, Automatic}, {Automatic, 8}}, +WindowMargins->{{83, Automatic}, {Automatic, 8}}, Magnification:>0.9 Inherited, FrontEndVersion->"12.1 for Mac OS X x86 (64-bit) (June 19, 2020)", StyleDefinitions->"Default.nb", @@ -1724,46 +1508,46 @@ CellTagsIndex->{} (*NotebookFileOutline Notebook[{ Cell[558, 20, 230, 4, 31, "Text",ExpressionUUID->"039b787c-64fd-4dfb-8011-578c383c402e"], -Cell[791, 26, 7530, 234, 822, "Input",ExpressionUUID->"9382707b-2876-460d-b0fb-ca70e05b1e9a"], -Cell[8324, 262, 12651, 328, 427, "Input",ExpressionUUID->"93a49805-7b39-4e71-a5ce-78f2b77ae9c0"], -Cell[20978, 592, 1003, 22, 238, "Text",ExpressionUUID->"0616ae41-3a02-473a-878f-96412e2fc8c5"], -Cell[21984, 616, 284, 7, 52, "Text",ExpressionUUID->"407acd61-eed1-4280-b150-0da33c32906a"], +Cell[791, 26, 7580, 235, 822, "Input",ExpressionUUID->"9382707b-2876-460d-b0fb-ca70e05b1e9a"], +Cell[8374, 263, 13215, 342, 446, "Input",ExpressionUUID->"93a49805-7b39-4e71-a5ce-78f2b77ae9c0"], +Cell[21592, 607, 1309, 26, 238, "Text",ExpressionUUID->"0616ae41-3a02-473a-878f-96412e2fc8c5"], +Cell[22904, 635, 284, 7, 52, "Text",ExpressionUUID->"407acd61-eed1-4280-b150-0da33c32906a"], Cell[CellGroupData[{ -Cell[22293, 627, 886, 21, 66, "Input",ExpressionUUID->"f08feb2e-6079-473b-b866-92670dea0a56"], -Cell[23182, 650, 4779, 114, 134, "Output",ExpressionUUID->"fb0d3b69-6fc5-442b-8456-51f7f65e0c32"] +Cell[23213, 646, 935, 22, 66, "Input",ExpressionUUID->"f08feb2e-6079-473b-b866-92670dea0a56"], +Cell[24151, 670, 5903, 142, 154, "Output",ExpressionUUID->"faa343c3-02f2-4fc4-b2b0-2208fa60cf35"] }, Open ]], -Cell[27976, 767, 247, 4, 31, "Text",ExpressionUUID->"f1684154-b22e-4827-98da-4e4dad150835"], +Cell[30069, 815, 247, 4, 31, "Text",ExpressionUUID->"f1684154-b22e-4827-98da-4e4dad150835"], Cell[CellGroupData[{ -Cell[28248, 775, 1508, 41, 144, "Input",ExpressionUUID->"217d7885-1018-483e-8952-08082feb5695"], -Cell[29759, 818, 864, 20, 54, "Output",ExpressionUUID->"c8c1ef8b-557d-4acc-8854-09d94a878b1a"] +Cell[30341, 823, 1658, 43, 144, "Input",ExpressionUUID->"217d7885-1018-483e-8952-08082feb5695"], +Cell[32002, 868, 2267, 61, 97, "Output",ExpressionUUID->"2e4b2150-6f82-4c7d-8607-3649724667c6"] }, Open ]], Cell[CellGroupData[{ -Cell[30660, 843, 945, 22, 85, "Input",ExpressionUUID->"58c30e25-83c4-4d05-ad21-e88fcb7f4181"], -Cell[31608, 867, 11921, 332, 299, "Output",ExpressionUUID->"480d0f52-423a-4af8-96f6-25e0827fcb2a"] +Cell[34306, 934, 1042, 23, 85, "Input",ExpressionUUID->"58c30e25-83c4-4d05-ad21-e88fcb7f4181"], +Cell[35351, 959, 568, 11, 50, "Output",ExpressionUUID->"333c90a7-66b2-4ebd-90da-2deda377e542"] }, Open ]], -Cell[43544, 1202, 160, 3, 31, "Text",ExpressionUUID->"5c00d19c-efbb-475f-a4a4-adc1877059e7"], -Cell[43707, 1207, 5502, 128, 275, "Input",ExpressionUUID->"013ba19d-162c-4e14-9e6b-43bcf125ceef"], -Cell[49212, 1337, 531, 9, 31, "Text",ExpressionUUID->"ccf44efd-e09a-4bbb-a696-b4c90f7a95eb"], +Cell[35934, 973, 229, 4, 31, "Text",ExpressionUUID->"5c00d19c-efbb-475f-a4a4-adc1877059e7"], +Cell[36166, 979, 5502, 128, 275, "Input",ExpressionUUID->"013ba19d-162c-4e14-9e6b-43bcf125ceef"], +Cell[41671, 1109, 820, 19, 218, "Text",ExpressionUUID->"ccf44efd-e09a-4bbb-a696-b4c90f7a95eb"], Cell[CellGroupData[{ -Cell[49768, 1350, 948, 18, 47, "Input",ExpressionUUID->"b4fd3303-7c30-4af8-94b1-4505577eaf40"], -Cell[50719, 1370, 636, 10, 31, "Output",ExpressionUUID->"3c247a78-5f1f-4bfe-aeb8-c0def9d99fe4"] +Cell[42516, 1132, 1040, 20, 66, "Input",ExpressionUUID->"b4fd3303-7c30-4af8-94b1-4505577eaf40"], +Cell[43559, 1154, 636, 10, 31, "Output",ExpressionUUID->"3c247a78-5f1f-4bfe-aeb8-c0def9d99fe4"] }, Open ]], -Cell[51370, 1383, 182, 3, 31, "Text",ExpressionUUID->"6e1bb727-a1dc-4c24-af7f-abfa0f3b9409"], +Cell[44210, 1167, 182, 3, 31, "Text",ExpressionUUID->"6e1bb727-a1dc-4c24-af7f-abfa0f3b9409"], Cell[CellGroupData[{ -Cell[51577, 1390, 952, 24, 66, "Input",ExpressionUUID->"c24549d5-e55a-4ad4-a91d-4e532e8d2688"], -Cell[52532, 1416, 781, 12, 108, "Output",ExpressionUUID->"8a1b58c3-1441-4fe3-bbe7-d56171dd7cae"] +Cell[44417, 1174, 952, 24, 66, "Input",ExpressionUUID->"c24549d5-e55a-4ad4-a91d-4e532e8d2688"], +Cell[45372, 1200, 781, 12, 108, "Output",ExpressionUUID->"8a1b58c3-1441-4fe3-bbe7-d56171dd7cae"] }, Open ]], Cell[CellGroupData[{ -Cell[53350, 1433, 812, 21, 66, "Input",ExpressionUUID->"27add3ed-17eb-49e9-9b82-a98025fa047d"], -Cell[54165, 1456, 1254, 19, 203, "Output",ExpressionUUID->"9edd45ed-0ce4-41cc-9909-2527fddc394d"] +Cell[46190, 1217, 812, 21, 66, "Input",ExpressionUUID->"27add3ed-17eb-49e9-9b82-a98025fa047d"], +Cell[47005, 1240, 1254, 19, 203, "Output",ExpressionUUID->"9edd45ed-0ce4-41cc-9909-2527fddc394d"] }, Open ]], Cell[CellGroupData[{ -Cell[55456, 1480, 1812, 44, 156, "Input",ExpressionUUID->"0d5d8425-f226-4e6a-91fd-34a5822f73b9"], -Cell[57271, 1526, 2994, 80, 179, "Output",ExpressionUUID->"1dcd2096-117e-4fa7-8a27-82d21901823d"] +Cell[48296, 1264, 1812, 44, 156, "Input",ExpressionUUID->"0d5d8425-f226-4e6a-91fd-34a5822f73b9"], +Cell[50111, 1310, 2994, 80, 155, "Output",ExpressionUUID->"1dcd2096-117e-4fa7-8a27-82d21901823d"] }, Open ]], Cell[CellGroupData[{ -Cell[60302, 1611, 764, 21, 46, "Input",ExpressionUUID->"4f3c940a-b41f-43f3-9aa8-8115c96b6bee"], -Cell[61069, 1634, 2830, 70, 76, "Output",ExpressionUUID->"f8a67d4a-9073-4868-948b-275b14bd35f8"] +Cell[53142, 1395, 764, 21, 46, "Input",ExpressionUUID->"4f3c940a-b41f-43f3-9aa8-8115c96b6bee"], +Cell[53909, 1418, 2830, 70, 76, "Output",ExpressionUUID->"f8a67d4a-9073-4868-948b-275b14bd35f8"] }, Open ]] } ] diff --git a/src/step2.cpp b/src/step2.cpp index a41b7f64..af37fa65 100644 --- a/src/step2.cpp +++ b/src/step2.cpp @@ -51,21 +51,22 @@ bool RuckigStep2::time_up_acc1_vel(Profile& profile, double vMax, double aMax, d // Profile UDDU { std::array polynom; - polynom[0] = 12*Power(jMax,4); - polynom[1] = 24*(2*a0 + aMax)*Power(jMax,3); - polynom[2] = 12*Power(jMax,2)*(5*Power(a0,2) + 4*a0*aMax + Power(aMax,2) - 2*aMax*jMax*tf + 2*jMax*(v0 - vf)); - polynom[3] = 24*a0*jMax*(Power(a0,2) + a0*aMax + Power(aMax,2) - 2*aMax*jMax*tf + 2*jMax*(v0 - vf)); - polynom[4] = 3*Power(a0,4) + 4*Power(a0,3)*aMax + 6*Power(a0,2)*(Power(aMax,2) - 2*aMax*jMax*tf + 2*jMax*(v0 - vf)) + 12*jMax*(-2*aMax*jMax*(p0 - pf + tf*v0) + Power(aMax,2)*(v0 - vf) + jMax*Power(v0 - vf,2)); + + polynom[0] = 1.0; + polynom[1] = (2*(2*a0 + aMax))/jMax; + polynom[2] = (5*Power(a0,2) + Power(af,2) + 4*a0*aMax + 2*af*aMax + Power(aMax,2) - 2*aMax*jMax*tf + 2*jMax*v0 - 2*jMax*vf)/Power(jMax,2); + polynom[3] = (2*a0*(Power(a0,2) + Power(af,2) + a0*aMax + 2*af*aMax + Power(aMax,2) - 2*aMax*jMax*tf + 2*jMax*v0 - 2*jMax*vf))/Power(jMax,3); + polynom[4] = (3*Power(a0,4) + 3*Power(af,4) + 4*Power(a0,3)*aMax + 8*Power(af,3)*aMax + 6*Power(af,2)*(Power(aMax,2) + 2*jMax*(v0 - vf)) + 12*jMax*(-2*aMax*jMax*(p0 - pf + tf*v0) + Power(aMax,2)*(v0 - vf) + jMax*Power(v0 - vf,2)) + 24*af*aMax*jMax*(v0 - vf) + 6*Power(a0,2)*(Power(af,2) + 2*af*aMax + Power(aMax,2) - 2*aMax*jMax*tf + 2*jMax*v0 - 2*jMax*vf))/(12.*Power(jMax,4)); auto roots = Roots::solveQuart(polynom); for (double t: roots) { profile.t[0] = t; profile.t[1] = 0; profile.t[2] = a0/jMax + t; - profile.t[3] = -(Power(a0,2) + 2*a0*(aMax + 2*jMax*t) + 2*(Power(aMax,2) + aMax*jMax*(2*t - tf) + jMax*(jMax*Power(t,2) + v0 - vf)))/(2.*aMax*jMax); + profile.t[3] = -(Power(a0,2) + Power(af,2) + 2*a0*(aMax + 2*jMax*t) + 2*(af*aMax + Power(aMax,2) + aMax*jMax*(2*t - tf) + jMax*(jMax*Power(t,2) + v0 - vf)))/(2.*aMax*jMax); profile.t[4] = aMax/jMax; - profile.t[5] = (Power(a0,2) - 2*Power(aMax,2) + 4*a0*jMax*t + 2*jMax*(jMax*Power(t,2) + v0 - vf))/(2.*aMax*jMax); - profile.t[6] = profile.t[4]; + profile.t[5] = (Power(a0,2) + Power(af,2) - 2*Power(aMax,2) + 4*a0*jMax*t + 2*jMax*(jMax*Power(t,2) + v0 - vf))/(2.*aMax*jMax); + profile.t[6] = (af + aMax)/jMax; profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, -jMax, 0, jMax}); if (profile.check(tf, pf, vf, af, vMax, aMax)) { @@ -77,21 +78,21 @@ bool RuckigStep2::time_up_acc1_vel(Profile& profile, double vMax, double aMax, d // Profile UDUD { std::array polynom; - polynom[0] = 12*Power(jMax,4); - polynom[1] = 24*(2*a0 - aMax)*Power(jMax,3); - polynom[2] = 12*Power(jMax,2)*(5*Power(a0,2) - 4*a0*aMax - Power(aMax,2) + 2*aMax*jMax*tf + 2*jMax*(v0 - vf)); - polynom[3] = 24*a0*jMax*(Power(a0,2) - a0*aMax - Power(aMax,2) + 2*aMax*jMax*tf + 2*jMax*(v0 - vf)); - polynom[4] = 3*Power(a0,4) - 4*Power(a0,3)*aMax + 12*jMax*(2*aMax*jMax*(p0 - pf + tf*v0) + jMax*Power(v0 - vf,2) + Power(aMax,2)*(-v0 + vf)) - 6*Power(a0,2)*(Power(aMax,2) - 2*aMax*jMax*tf + 2*jMax*(-v0 + vf)); + polynom[0] = 1.0; + polynom[1] = (4*a0 - 2*aMax)/jMax; + polynom[2] = -((-5*Power(a0,2) + Power(af,2) + 4*a0*aMax - 2*af*aMax + Power(aMax,2) - 2*aMax*jMax*tf - 2*jMax*v0 + 2*jMax*vf)/Power(jMax,2)); + polynom[3] = (2*a0*(Power(a0,2) - Power(af,2) - a0*aMax + 2*af*aMax - Power(aMax,2) + 2*aMax*jMax*tf + 2*jMax*v0 - 2*jMax*vf))/Power(jMax,3); + polynom[4] = (3*Power(a0,4) + 3*Power(af,4) - 4*Power(a0,3)*aMax - 8*Power(af,3)*aMax + 24*af*aMax*jMax*(v0 - vf) - 6*Power(a0,2)*(Power(af,2) - 2*af*aMax + Power(aMax,2) - 2*aMax*jMax*tf - 2*jMax*v0 + 2*jMax*vf) + 12*jMax*(2*aMax*jMax*(p0 - pf + tf*v0) + jMax*Power(v0 - vf,2) + Power(aMax,2)*(-v0 + vf)) + 6*Power(af,2)*(Power(aMax,2) + 2*jMax*(-v0 + vf)))/(12.*Power(jMax,4)); auto roots = Roots::solveQuart(polynom); for (double t: roots) { profile.t[0] = t; profile.t[1] = 0; profile.t[2] = a0/jMax + t; - profile.t[3] = (Power(a0,2) - 2*Power(aMax,2) - 2*a0*(aMax - 2*jMax*t) + 2*aMax*jMax*(-2*t + tf) + 2*jMax*(jMax*Power(t,2) + v0 - vf))/(2.*aMax*jMax); + profile.t[3] = (Power(a0,2) - Power(af,2) - 2*a0*aMax + 2*af*aMax - 2*Power(aMax,2) + 4*a0*jMax*t - 4*aMax*jMax*t + 2*Power(jMax,2)*Power(t,2) + 2*aMax*jMax*tf + 2*jMax*v0 - 2*jMax*vf)/(2.*aMax*jMax); profile.t[4] = aMax/jMax; - profile.t[5] = -(Power(a0,2) + 4*a0*jMax*t + 2*(Power(aMax,2) + jMax*(jMax*Power(t,2) + v0 - vf)))/(2.*aMax*jMax); - profile.t[6] = profile.t[4]; + profile.t[5] = -(Power(a0,2) - Power(af,2) + 4*a0*jMax*t + 2*(Power(aMax,2) + jMax*(jMax*Power(t,2) + v0 - vf)))/(2.*aMax*jMax); + profile.t[6] = (-af + aMax)/jMax; profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, jMax, 0, -jMax}); if (profile.check(tf, pf, vf, af, vMax, aMax)) { @@ -162,8 +163,6 @@ bool RuckigStep2::time_up_acc0_vel(Profile& profile, double vMax, double aMax, d bool RuckigStep2::time_up_vel(Profile& profile, double vMax, double aMax, double jMax) { // Profile UDDU { - // std::cout << "HERE" << a0 << " " << tf << std::endl; - // Find root of 5th order polynom std::array polynom; polynom[0] = 1.0; @@ -196,7 +195,6 @@ bool RuckigStep2::time_up_vel(Profile& profile, double vMax, double aMax, double // 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 << " " << Roots::polyEval(deriv, tz) << std::endl; if (std::abs(val_new) < 1e-15) { // if (val_current * val_new < 0) { // tz_intervals.insert({tz_current, tz}); @@ -221,8 +219,6 @@ bool RuckigStep2::time_up_vel(Profile& profile, double vMax, double aMax, double double upper = std::get<1>(interval); double t = Roots::shrinkInterval(polynom, lower, upper, 2e-16); - // std::cout << std::setprecision(15) << t << " " << Roots::polyEval(polynom, t) << std::endl; - double vPlat = Power(a0,2)/(2.*jMax) + 2*a0*t + jMax*Power(t,2) + v0; double h1 = 2*Sqrt((vPlat - vf)/jMax); @@ -239,16 +235,6 @@ bool RuckigStep2::time_up_vel(Profile& profile, double vMax, double aMax, double profile.t[5] = 0; profile.t[6] = profile.t[4]; - // std::cout << profile.t[0] + profile.t[2] + profile.t[3] + profile.t[4] + profile.t[6] << " " << tf << " " << (6.*jMax*(Power(a0,2) + 4*a0*jMax*t + 2*jMax*(jMax*Power(t,2) + v0))) << 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; - // std::cout << "---" << std::endl; - profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, -jMax, 0, jMax}); if (profile.check(tf, pf, vf, af, vMax, aMax)) { return true; @@ -347,7 +333,7 @@ bool RuckigStep2::time_up_vel(Profile& profile, double vMax, double aMax, double } bool RuckigStep2::time_up_acc0_acc1(Profile& profile, double vMax, double aMax, double jMax) { - if (std::abs(a0) < DBL_EPSILON) { + if (std::abs(a0) < DBL_EPSILON && std::abs(af) < DBL_EPSILON) { profile.t[0] = (Power(aMax,2)*Power(tf,2) - Power(v0 - vf,2) + 2*aMax*(2*p0 - 2*pf + tf*(v0 + vf)))/(2.*Power(aMax,2)*tf); profile.t[1] = -(Power(aMax,2)*Power(tf,2) - 2*Power(v0 - vf,2) + aMax*(8*p0 - 8*pf + 5*tf*v0 + 3*tf*vf))/(2.*Power(aMax,2)*tf); profile.t[2] = profile.t[0]; @@ -357,29 +343,24 @@ bool RuckigStep2::time_up_acc0_acc1(Profile& profile, double vMax, double aMax, profile.t[6] = profile.t[0]; jMax = aMax/profile.t[0]; - // std::cout << profile.t[0] << std::endl; - // std::cout << jMax << std::endl; - profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, -jMax, 0, jMax}); return profile.check(tf, pf, vf, af, vMax, aMax); } - - double h1 = -12*(2*Power(aMax,3)*tf + Power(a0,2)*(aMax*tf - v0 + vf) - 2*a0*aMax*(aMax*tf - v0 + vf)); - double h2 = -12*(Power(aMax,2)*Power(tf,2) - Power(v0 - vf,2) + 2*aMax*(2*p0 - 2*pf + tf*(v0 + vf))); - double h3 = 3*Power(a0,3) - 4*Power(a0,2)*aMax - 12*a0*Power(aMax,2) + 24*Power(aMax,3); - double h4 = h1 - Sqrt(Power(h1,2) - 4*a0*h2*h3); - double h6 = 2*aMax*h4*(2*p0 - 2*pf + tf*(v0 + vf)); - double h7 = 2*a0*aMax*h2*(a0*tf + 2*v0 - 2*vf); - - profile.t[0] = (6*(a0 - aMax)*(-h6 + h7 + 4*Power(aMax,3)*h2*tf - Power(aMax,2)*tf*(4*a0*h2 + h4*tf) + (-2*Power(a0,2)*h2 + h4*(v0 - vf))*(v0 - vf)))/(a0*h2*h3); - profile.t[1] = (24*Power(aMax,2)*(-h6 + 4*Power(aMax,3)*h2*tf - Power(aMax,2)*h4*Power(tf,2) + h4*Power(v0 - vf,2)) - 6*a0*aMax*(-h6 + 16*Power(aMax,3)*h2*tf - Power(aMax,2)*(h4*Power(tf,2) + 12*h2*(v0 - vf)) + h4*Power(v0 - vf,2)) - 3*Power(a0,4)*h2*(aMax*tf - v0 + vf) - 4*Power(a0,3)*aMax*h2*(aMax*tf - v0 + vf) + 3*Power(a0,2)*(h6 + 16*Power(aMax,3)*h2*tf - h4*Power(v0 - vf,2) + Power(aMax,2)*(h4*Power(tf,2) + 20*h2*(-v0 + vf))))/(2.*a0*aMax*h2*h3); - profile.t[2] = (-6*aMax*(-h6 + h7 + 4*Power(aMax,3)*h2*tf - Power(aMax,2)*tf*(4*a0*h2 + h4*tf) + (-2*Power(a0,2)*h2 + h4*(v0 - vf))*(v0 - vf)))/(a0*h2*h3); + + double h3 = Sqrt(aMax*(6*Power(aMax,5)*Power(tf,2) + 3*Power(af,4)*(2*p0 - 2*pf + tf*(aMax*tf + 2*v0)) + 3*Power(a0,4)*(2*p0 - 2*pf + tf*(aMax*tf + 2*vf)) + 6*Power(af,2)*aMax*(Power(aMax,2)*Power(tf,2) + 2*Power(v0 - vf,2) + aMax*(-4*p0 + 4*pf + tf*v0 - 5*tf*vf)) + 4*Power(af,3)*(2*Power(aMax,2)*Power(tf,2) + Power(v0 - vf,2) + 2*aMax*(p0 - pf + 2*tf*v0 - tf*vf)) - 4*Power(a0,3)*(2*Power(aMax,2)*Power(tf,2) + Power(v0 - vf,2) + 2*aMax*(p0 - pf - tf*v0 + 2*tf*vf)) - 12*af*Power(aMax,2)*(-Power(v0 - vf,2) + aMax*(4*p0 - 4*pf + tf*v0 + 3*tf*vf)) - 6*Power(a0,2)*(Power(af,2)*(2*p0 - 2*pf + tf*(v0 + vf)) + 2*af*aMax*(2*p0 - 2*pf + tf*(v0 + vf)) - aMax*(Power(aMax,2)*Power(tf,2) + 2*Power(v0 - vf,2) + aMax*(-4*p0 + 4*pf - 5*tf*v0 + tf*vf))) + 12*a0*aMax*(Power(af,2)*(2*p0 - 2*pf + tf*(v0 + vf)) + 2*af*aMax*(2*p0 - 2*pf + tf*(v0 + vf)) + aMax*(-Power(v0 - vf,2) + aMax*(4*p0 - 4*pf + 3*tf*v0 + tf*vf))))); + double h1 = (3*Power(a0,2)*aMax*tf + 3*Power(af,2)*aMax*tf - 6*a0*Power(aMax,2)*tf + 6*af*Power(aMax,2)*tf + 6*Power(aMax,3)*tf - 3*Power(a0,2)*v0 + 3*Power(af,2)*v0 + 6*a0*aMax*v0 + 6*af*aMax*v0 + 3*Power(a0,2)*vf - 3*Power(af,2)*vf - 6*a0*aMax*vf - 6*af*aMax*vf - Sqrt(6)*h3); + double h5 = (3*Power(a0,3) - 3*Power(af,3) + Power(a0,2)*(3*af - 4*aMax) - 4*Power(af,2)*aMax + 12*af*Power(aMax,2) + 24*Power(aMax,3) - a0*(3*Power(af,2) + 16*af*aMax + 12*Power(aMax,2))); + double h2 = ((a0 - af)*h5); + double h4 = (2.*(a0 - af)*aMax*h5); + + profile.t[0] = (2*(a0 - aMax)*h1)/h2; + profile.t[1] = (-6*Power(a0,2)*Power(af,2)*aMax*tf + 6*Power(af,4)*aMax*tf - 4*Power(a0,3)*Power(aMax,2)*tf - 12*Power(a0,2)*af*Power(aMax,2)*tf + 16*Power(af,3)*Power(aMax,2)*tf + 18*Power(a0,2)*Power(aMax,3)*tf + 30*Power(af,2)*Power(aMax,3)*tf - 36*a0*Power(aMax,4)*tf + 36*af*Power(aMax,4)*tf + 48*Power(aMax,5)*tf + 4*Power(a0,3)*aMax*v0 - 12*a0*Power(af,2)*aMax*v0 + 8*Power(af,3)*aMax*v0 - 24*Power(a0,2)*Power(aMax,2)*v0 - 24*a0*af*Power(aMax,2)*v0 + 48*Power(af,2)*Power(aMax,2)*v0 + 24*a0*Power(aMax,3)*v0 + 72*af*Power(aMax,3)*v0 - 4*Power(a0,3)*aMax*vf + 12*a0*Power(af,2)*aMax*vf - 8*Power(af,3)*aMax*vf + 24*Power(a0,2)*Power(aMax,2)*vf + 24*a0*af*Power(aMax,2)*vf - 48*Power(af,2)*Power(aMax,2)*vf - 24*a0*Power(aMax,3)*vf - 72*af*Power(aMax,3)*vf + Sqrt(6)*Power(a0,2)*h3 - Sqrt(6)*Power(af,2)*h3 + 2*Sqrt(6)*a0*aMax*h3 - 2*Sqrt(6)*af*aMax*h3 - 8*Sqrt(6)*Power(aMax,2)*h3)/h4; + profile.t[2] = (-2*aMax*h1)/h2; profile.t[3] = 0; profile.t[4] = profile.t[2]; - profile.t[5] = (24*Power(aMax,2)*(-h6 + 4*Power(aMax,3)*h2*tf - Power(aMax,2)*h4*Power(tf,2) + h4*Power(v0 - vf,2)) - 6*a0*aMax*(-h6 + 16*Power(aMax,3)*h2*tf - Power(aMax,2)*(h4*Power(tf,2) + 20*h2*(v0 - vf)) + h4*Power(v0 - vf,2)) + 3*Power(a0,2)*(-h6 + 24*Power(aMax,3)*h2*tf - Power(aMax,2)*(h4*Power(tf,2) + 28*h2*(v0 - vf)) + h4*Power(v0 - vf,2)) + 3*Power(a0,4)*h2*(3*aMax*tf - v0 + vf) - 4*Power(a0,3)*aMax*h2*(7*aMax*tf - 5*v0 + 5*vf))/(2.*a0*aMax*h2*h3); - profile.t[6] = profile.t[2]; - - jMax = h4/(2*h2); + profile.t[5] = (6*Power(a0,4)*aMax*tf - 6*Power(a0,2)*Power(af,2)*aMax*tf - 16*Power(a0,3)*Power(aMax,2)*tf + 12*a0*Power(af,2)*Power(aMax,2)*tf + 4*Power(af,3)*Power(aMax,2)*tf + 30*Power(a0,2)*Power(aMax,3)*tf + 18*Power(af,2)*Power(aMax,3)*tf - 36*a0*Power(aMax,4)*tf + 36*af*Power(aMax,4)*tf + 48*Power(aMax,5)*tf + 8*Power(a0,3)*aMax*v0 - 12*Power(a0,2)*af*aMax*v0 + 4*Power(af,3)*aMax*v0 - 48*Power(a0,2)*Power(aMax,2)*v0 + 24*a0*af*Power(aMax,2)*v0 + 24*Power(af,2)*Power(aMax,2)*v0 + 72*a0*Power(aMax,3)*v0 + 24*af*Power(aMax,3)*v0 - 8*Power(a0,3)*aMax*vf + 12*Power(a0,2)*af*aMax*vf - 4*Power(af,3)*aMax*vf + 48*Power(a0,2)*Power(aMax,2)*vf - 24*a0*af*Power(aMax,2)*vf - 24*Power(af,2)*Power(aMax,2)*vf - 72*a0*Power(aMax,3)*vf - 24*af*Power(aMax,3)*vf - Sqrt(6)*Power(a0,2)*h3 + Sqrt(6)*Power(af,2)*h3 + 2*Sqrt(6)*a0*aMax*h3 - 2*Sqrt(6)*af*aMax*h3 - 8*Sqrt(6)*Power(aMax,2)*h3)/h4; + profile.t[6] = (-2*(af + aMax)*h1)/h2; + jMax = -(-12*Power(a0,2)*aMax*tf - 12*Power(af,2)*aMax*tf + 24*a0*Power(aMax,2)*tf - 24*af*Power(aMax,2)*tf - 24*Power(aMax,3)*tf + 12*Power(a0,2)*v0 - 12*Power(af,2)*v0 - 24*a0*aMax*v0 - 24*af*aMax*v0 - 12*Power(a0,2)*vf + 12*Power(af,2)*vf + 24*a0*aMax*vf + 24*af*aMax*vf - 4*Sqrt(9*Power(2*Power(aMax,3)*tf + Power(af,2)*(aMax*tf + v0 - vf) + 2*af*aMax*(aMax*tf + v0 - vf) + Power(a0,2)*(aMax*tf - v0 + vf) - 2*a0*aMax*(aMax*tf - v0 + vf),2) + 3*(a0 - af)*h5*(Power(aMax,2)*Power(tf,2) - Power(v0 - vf,2) + 2*aMax*(2*p0 - 2*pf + tf*(v0 + vf)))))/(24.*(Power(aMax,2)*Power(tf,2) - Power(v0 - vf,2) + 2*aMax*(2*p0 - 2*pf + tf*(v0 + vf)))); profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, -jMax, 0, jMax}); return profile.check(tf, pf, vf, af, vMax, aMax); @@ -390,13 +371,16 @@ bool RuckigStep2::time_up_acc1(Profile& profile, double vMax, double aMax, doubl // Case UDDU, Solution 2 { + double h1 = Sqrt(2)*Sqrt(Power(jMax,2)*(2*Power(Power(a0,3) - Power(af,3) + 3*Power(a0,2)*aMax + 3*a0*Power(aMax,2) + 3*Power(aMax,2)*jMax*tf - 3*af*aMax*(aMax - 2*jMax*tf) - 3*Power(af,2)*(aMax - jMax*tf) - 3*Power(jMax,2)*(2*p0 - 2*pf + aMax*Power(tf,2) + 2*tf*vf),2) - 3*(Power(a0,2) + Power(af,2) + 2*a0*aMax + 2*af*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*v0 - jMax*vf))*(Power(a0,4) + 3*Power(af,4) + 4*Power(a0,3)*aMax + 8*Power(af,3)*aMax + 6*Power(a0,2)*Power(aMax,2) + 6*Power(af,2)*(Power(aMax,2) + 2*jMax*(v0 - vf)) + 12*jMax*(-2*aMax*jMax*(p0 - pf + tf*v0) + Power(aMax,2)*(v0 - vf) + jMax*Power(v0 - vf,2)) + 24*af*aMax*jMax*(v0 - vf) - 4*a0*(Power(af,3) + 3*af*aMax*(aMax - 2*jMax*tf) + 3*Power(af,2)*(aMax - jMax*tf) + 3*jMax*(-(Power(aMax,2)*tf) + jMax*(2*p0 - 2*pf + aMax*Power(tf,2) + 2*tf*vf)))))); + double h2 = (6.*Power(jMax,2)*(Power(a0,2) + Power(af,2) + 2*a0*aMax + 2*af*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*v0 - jMax*vf))); + profile.t[0] = 0; profile.t[1] = 0; - profile.t[2] = -(-2*Power(a0,3)*jMax - 6*Power(a0,2)*aMax*jMax - 6*a0*Power(aMax,2)*jMax + 12*Power(jMax,3)*p0 - 12*Power(jMax,3)*pf - 6*Power(aMax,2)*Power(jMax,2)*tf + 6*aMax*Power(jMax,3)*Power(tf,2) + 12*Power(jMax,3)*tf*vf - Sqrt(2)*Sqrt(Power(jMax,2)*(2*Power(Power(a0,3) + 3*Power(a0,2)*aMax + 3*a0*Power(aMax,2) + 3*Power(aMax,2)*jMax*tf - 3*Power(jMax,2)*(2*p0 - 2*pf + tf*(aMax*tf + 2*vf)),2) - 3*(Power(a0,2) + 2*a0*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*(v0 - vf)))*(Power(a0,4) + 4*Power(a0,3)*aMax + 6*Power(a0,2)*Power(aMax,2) + 12*jMax*(-2*aMax*jMax*(p0 - pf + tf*v0) + Power(aMax,2)*(v0 - vf) + jMax*Power(v0 - vf,2)) - 12*a0*jMax*(-(Power(aMax,2)*tf) + jMax*(2*p0 - 2*pf + aMax*Power(tf,2) + 2*tf*vf))))))/(6.*Power(jMax,2)*(Power(a0,2) + 2*a0*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*(v0 - vf)))); - profile.t[3] = -(4*Power(a0,3)*jMax + 12*Power(a0,2)*aMax*jMax + 18*a0*Power(aMax,2)*jMax + 12*Power(aMax,3)*jMax + 12*Power(jMax,3)*p0 - 12*Power(jMax,3)*pf - 12*a0*aMax*Power(jMax,2)*tf - 18*Power(aMax,2)*Power(jMax,2)*tf + 6*aMax*Power(jMax,3)*Power(tf,2) + 12*a0*Power(jMax,2)*v0 + 12*aMax*Power(jMax,2)*v0 - 12*a0*Power(jMax,2)*vf - 12*aMax*Power(jMax,2)*vf + 12*Power(jMax,3)*tf*vf + Sqrt(2)*Sqrt(Power(jMax,2)*(2*Power(Power(a0,3) + 3*Power(a0,2)*aMax + 3*a0*Power(aMax,2) + 3*Power(aMax,2)*jMax*tf - 3*Power(jMax,2)*(2*p0 - 2*pf + tf*(aMax*tf + 2*vf)),2) - 3*(Power(a0,2) + 2*a0*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*(v0 - vf)))*(Power(a0,4) + 4*Power(a0,3)*aMax + 6*Power(a0,2)*Power(aMax,2) + 12*jMax*(-2*aMax*jMax*(p0 - pf + tf*v0) + Power(aMax,2)*(v0 - vf) + jMax*Power(v0 - vf,2)) - 12*a0*jMax*(-(Power(aMax,2)*tf) + jMax*(2*p0 - 2*pf + aMax*Power(tf,2) + 2*tf*vf))))))/(6.*Power(jMax,2)*(Power(a0,2) + 2*a0*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*(v0 - vf)))); - profile.t[4] = -(-4*Power(a0,3)*jMax - 12*Power(a0,2)*aMax*jMax - 18*a0*Power(aMax,2)*jMax - 12*Power(aMax,3)*jMax - 12*Power(jMax,3)*p0 + 12*Power(jMax,3)*pf + 12*a0*aMax*Power(jMax,2)*tf + 18*Power(aMax,2)*Power(jMax,2)*tf - 6*aMax*Power(jMax,3)*Power(tf,2) - 12*a0*Power(jMax,2)*v0 - 12*aMax*Power(jMax,2)*v0 + 12*a0*Power(jMax,2)*vf + 12*aMax*Power(jMax,2)*vf - 12*Power(jMax,3)*tf*vf + Sqrt(2)*Sqrt(Power(jMax,2)*(2*Power(Power(a0,3) + 3*Power(a0,2)*aMax + 3*a0*Power(aMax,2) + 3*Power(aMax,2)*jMax*tf - 3*Power(jMax,2)*(2*p0 - 2*pf + tf*(aMax*tf + 2*vf)),2) - 3*(Power(a0,2) + 2*a0*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*(v0 - vf)))*(Power(a0,4) + 4*Power(a0,3)*aMax + 6*Power(a0,2)*Power(aMax,2) + 12*jMax*(-2*aMax*jMax*(p0 - pf + tf*v0) + Power(aMax,2)*(v0 - vf) + jMax*Power(v0 - vf,2)) - 12*a0*jMax*(-(Power(aMax,2)*tf) + jMax*(2*p0 - 2*pf + aMax*Power(tf,2) + 2*tf*vf))))))/(6.*Power(jMax,2)*(Power(a0,2) + 2*a0*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*(v0 - vf)))); - profile.t[5] = -(2*Power(a0,3)*jMax + 12*Power(a0,2)*aMax*jMax + 18*a0*Power(aMax,2)*jMax + 12*Power(aMax,3)*jMax - 12*Power(jMax,3)*p0 + 12*Power(jMax,3)*pf - 6*Power(a0,2)*Power(jMax,2)*tf - 12*a0*aMax*Power(jMax,2)*tf - 18*Power(aMax,2)*Power(jMax,2)*tf + 6*aMax*Power(jMax,3)*Power(tf,2) + 12*aMax*Power(jMax,2)*v0 - 12*Power(jMax,3)*tf*v0 - 12*aMax*Power(jMax,2)*vf - Sqrt(2)*Sqrt(Power(jMax,2)*(2*Power(Power(a0,3) + 3*Power(a0,2)*aMax + 3*a0*Power(aMax,2) + 3*Power(aMax,2)*jMax*tf - 3*Power(jMax,2)*(2*p0 - 2*pf + tf*(aMax*tf + 2*vf)),2) - 3*(Power(a0,2) + 2*a0*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*(v0 - vf)))*(Power(a0,4) + 4*Power(a0,3)*aMax + 6*Power(a0,2)*Power(aMax,2) + 12*jMax*(-2*aMax*jMax*(p0 - pf + tf*v0) + Power(aMax,2)*(v0 - vf) + jMax*Power(v0 - vf,2)) - 12*a0*jMax*(-(Power(aMax,2)*tf) + jMax*(2*p0 - 2*pf + aMax*Power(tf,2) + 2*tf*vf))))))/(6.*Power(jMax,2)*(Power(a0,2) + 2*a0*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*(v0 - vf)))); - profile.t[6] = (aMax/jMax); + profile.t[2] = -(-2*Power(a0,3)*jMax + 2*Power(af,3)*jMax - 6*Power(a0,2)*aMax*jMax + 6*Power(af,2)*aMax*jMax - 6*a0*Power(aMax,2)*jMax + 6*af*Power(aMax,2)*jMax + 12*Power(jMax,3)*p0 - 12*Power(jMax,3)*pf - 6*Power(af,2)*Power(jMax,2)*tf - 12*af*aMax*Power(jMax,2)*tf - 6*Power(aMax,2)*Power(jMax,2)*tf + 6*aMax*Power(jMax,3)*Power(tf,2) + 12*Power(jMax,3)*tf*vf - h1)/h2; + profile.t[3] = -(4*Power(a0,3)*jMax + 6*a0*Power(af,2)*jMax + 2*Power(af,3)*jMax + 12*Power(a0,2)*aMax*jMax + 12*a0*af*aMax*jMax + 12*Power(af,2)*aMax*jMax + 18*a0*Power(aMax,2)*jMax + 18*af*Power(aMax,2)*jMax + 12*Power(aMax,3)*jMax + 12*Power(jMax,3)*p0 - 12*Power(jMax,3)*pf - 6*Power(af,2)*Power(jMax,2)*tf - 12*a0*aMax*Power(jMax,2)*tf - 12*af*aMax*Power(jMax,2)*tf - 18*Power(aMax,2)*Power(jMax,2)*tf + 6*aMax*Power(jMax,3)*Power(tf,2) + 12*a0*Power(jMax,2)*v0 + 12*aMax*Power(jMax,2)*v0 - 12*a0*Power(jMax,2)*vf - 12*aMax*Power(jMax,2)*vf + 12*Power(jMax,3)*tf*vf + h1)/h2; + profile.t[4] = -(-4*Power(a0,3)*jMax - 6*a0*Power(af,2)*jMax - 2*Power(af,3)*jMax - 12*Power(a0,2)*aMax*jMax - 12*a0*af*aMax*jMax - 12*Power(af,2)*aMax*jMax - 18*a0*Power(aMax,2)*jMax - 18*af*Power(aMax,2)*jMax - 12*Power(aMax,3)*jMax - 12*Power(jMax,3)*p0 + 12*Power(jMax,3)*pf + 6*Power(af,2)*Power(jMax,2)*tf + 12*a0*aMax*Power(jMax,2)*tf + 12*af*aMax*Power(jMax,2)*tf + 18*Power(aMax,2)*Power(jMax,2)*tf - 6*aMax*Power(jMax,3)*Power(tf,2) - 12*a0*Power(jMax,2)*v0 - 12*aMax*Power(jMax,2)*v0 + 12*a0*Power(jMax,2)*vf + 12*aMax*Power(jMax,2)*vf - 12*Power(jMax,3)*tf*vf + h1)/h2; + profile.t[5] = -(2*Power(a0,3)*jMax + 6*Power(a0,2)*af*jMax + 4*Power(af,3)*jMax + 12*Power(a0,2)*aMax*jMax + 12*a0*af*aMax*jMax + 12*Power(af,2)*aMax*jMax + 18*a0*Power(aMax,2)*jMax + 18*af*Power(aMax,2)*jMax + 12*Power(aMax,3)*jMax - 12*Power(jMax,3)*p0 + 12*Power(jMax,3)*pf - 6*Power(a0,2)*Power(jMax,2)*tf - 12*a0*aMax*Power(jMax,2)*tf - 12*af*aMax*Power(jMax,2)*tf - 18*Power(aMax,2)*Power(jMax,2)*tf + 6*aMax*Power(jMax,3)*Power(tf,2) + 12*af*Power(jMax,2)*v0 + 12*aMax*Power(jMax,2)*v0 - 12*Power(jMax,3)*tf*v0 - 12*af*Power(jMax,2)*vf - 12*aMax*Power(jMax,2)*vf - h1)/h2; + profile.t[6] = (af + aMax)/jMax; profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, -jMax, 0, jMax}); if (profile.check(tf, pf, vf, af, vMax, aMax)) { @@ -406,13 +390,16 @@ bool RuckigStep2::time_up_acc1(Profile& profile, double vMax, double aMax, doubl // Case UDUD, Solution 1 { + double h1 = Sqrt(2)*Sqrt(Power(jMax,2)*(2*Power(-Power(a0,3) + Power(af,3) + 3*Power(a0,2)*aMax - 3*a0*Power(aMax,2) + 3*af*aMax*(aMax - 2*jMax*tf) - 3*Power(af,2)*(aMax - jMax*tf) + 3*jMax*(Power(aMax,2)*tf + jMax*(2*p0 - 2*pf - aMax*Power(tf,2) + 2*tf*vf)),2) - 3*(Power(a0,2) - Power(af,2) - 2*a0*aMax + 2*af*aMax + 2*jMax*(aMax*tf + v0 - vf))*(Power(a0,4) + 3*Power(af,4) - 4*Power(a0,3)*aMax - 8*Power(af,3)*aMax + 6*Power(a0,2)*Power(aMax,2) + 24*af*aMax*jMax*(v0 - vf) + 12*jMax*(2*aMax*jMax*(p0 - pf + tf*v0) + jMax*Power(v0 - vf,2) + Power(aMax,2)*(-v0 + vf)) + 6*Power(af,2)*(Power(aMax,2) + 2*jMax*(-v0 + vf)) - 4*a0*(Power(af,3) + 3*af*aMax*(aMax - 2*jMax*tf) - 3*Power(af,2)*(aMax - jMax*tf) + 3*jMax*(Power(aMax,2)*tf + jMax*(2*p0 - 2*pf - aMax*Power(tf,2) + 2*tf*vf)))))); + double h2 = (6.*Power(jMax,2)*(Power(a0,2) - Power(af,2) - 2*a0*aMax + 2*af*aMax + 2*jMax*(aMax*tf + v0 - vf))); + profile.t[0] = 0; profile.t[1] = 0; - profile.t[2] = -(-2*Power(a0,3)*jMax + 6*Power(a0,2)*aMax*jMax - 6*a0*Power(aMax,2)*jMax + 12*Power(jMax,3)*p0 - 12*Power(jMax,3)*pf + 6*Power(aMax,2)*Power(jMax,2)*tf - 6*aMax*Power(jMax,3)*Power(tf,2) + 12*Power(jMax,3)*tf*vf + Sqrt(2)*Sqrt(Power(jMax,2)*(2*Power(Power(a0,3) - 3*Power(a0,2)*aMax + 3*a0*Power(aMax,2) - 3*jMax*(Power(aMax,2)*tf + jMax*(2*p0 - 2*pf - aMax*Power(tf,2) + 2*tf*vf)),2) - 3*(Power(a0,2) - 2*a0*aMax + 2*jMax*(aMax*tf + v0 - vf))*(Power(a0,4) - 4*Power(a0,3)*aMax + 6*Power(a0,2)*Power(aMax,2) + 12*jMax*(2*aMax*jMax*(p0 - pf + tf*v0) + jMax*Power(v0 - vf,2) + Power(aMax,2)*(-v0 + vf)) - 12*a0*jMax*(Power(aMax,2)*tf + jMax*(2*p0 - 2*pf - aMax*Power(tf,2) + 2*tf*vf))))))/(6.*Power(jMax,2)*(Power(a0,2) - 2*a0*aMax + 2*jMax*(aMax*tf + v0 - vf))); - profile.t[3] = (Sqrt(2)*Sqrt(Power(jMax,2)*(2*Power(Power(a0,3) - 3*Power(a0,2)*aMax + 3*a0*Power(aMax,2) - 3*jMax*(Power(aMax,2)*tf + jMax*(2*p0 - 2*pf - aMax*Power(tf,2) + 2*tf*vf)),2) - 3*(Power(a0,2) - 2*a0*aMax + 2*jMax*(aMax*tf + v0 - vf))*(Power(a0,4) - 4*Power(a0,3)*aMax + 6*Power(a0,2)*Power(aMax,2) + 12*jMax*(2*aMax*jMax*(p0 - pf + tf*v0) + jMax*Power(v0 - vf,2) + Power(aMax,2)*(-v0 + vf)) - 12*a0*jMax*(Power(aMax,2)*tf + jMax*(2*p0 - 2*pf - aMax*Power(tf,2) + 2*tf*vf))))))/(3.*Power(jMax,2)*(Power(a0,2) - 2*a0*aMax + 2*jMax*(aMax*tf + v0 - vf))); - profile.t[4] = -(4*Power(a0,3)*jMax - 12*Power(a0,2)*aMax*jMax + 6*a0*Power(aMax,2)*jMax + 12*Power(jMax,3)*p0 - 12*Power(jMax,3)*pf + 12*a0*aMax*Power(jMax,2)*tf - 6*Power(aMax,2)*Power(jMax,2)*tf - 6*aMax*Power(jMax,3)*Power(tf,2) + 12*a0*Power(jMax,2)*v0 - 12*aMax*Power(jMax,2)*v0 - 12*a0*Power(jMax,2)*vf + 12*aMax*Power(jMax,2)*vf + 12*Power(jMax,3)*tf*vf + Sqrt(2)*Sqrt(Power(jMax,2)*(2*Power(Power(a0,3) - 3*Power(a0,2)*aMax + 3*a0*Power(aMax,2) - 3*jMax*(Power(aMax,2)*tf + jMax*(2*p0 - 2*pf - aMax*Power(tf,2) + 2*tf*vf)),2) - 3*(Power(a0,2) - 2*a0*aMax + 2*jMax*(aMax*tf + v0 - vf))*(Power(a0,4) - 4*Power(a0,3)*aMax + 6*Power(a0,2)*Power(aMax,2) + 12*jMax*(2*aMax*jMax*(p0 - pf + tf*v0) + jMax*Power(v0 - vf,2) + Power(aMax,2)*(-v0 + vf)) - 12*a0*jMax*(Power(aMax,2)*tf + jMax*(2*p0 - 2*pf - aMax*Power(tf,2) + 2*tf*vf))))))/(6.*Power(jMax,2)*(Power(a0,2) - 2*a0*aMax + 2*jMax*(aMax*tf + v0 - vf))); - profile.t[5] = (Power(a0,3) + Power(a0,2)*(-6*aMax + 3*jMax*tf) + 6*a0*(Power(aMax,2) + jMax*(v0 - vf)) - 6*jMax*(aMax*(aMax*tf + 2*v0 - 2*vf) - jMax*(2*p0 - 2*pf + tf*(v0 + vf))))/(3.*jMax*(Power(a0,2) - 2*a0*aMax + 2*jMax*(aMax*tf + v0 - vf))); - profile.t[6] = (aMax/jMax); + profile.t[2] = -(-2*Power(a0,3)*jMax + 2*Power(af,3)*jMax + 6*Power(a0,2)*aMax*jMax - 6*Power(af,2)*aMax*jMax - 6*a0*Power(aMax,2)*jMax + 6*af*Power(aMax,2)*jMax + 12*Power(jMax,3)*p0 - 12*Power(jMax,3)*pf + 6*Power(af,2)*Power(jMax,2)*tf - 12*af*aMax*Power(jMax,2)*tf + 6*Power(aMax,2)*Power(jMax,2)*tf - 6*aMax*Power(jMax,3)*Power(tf,2) + 12*Power(jMax,3)*tf*vf + h1)/h2; + profile.t[3] = (h1)/(3.*Power(jMax,2)*(Power(a0,2) - Power(af,2) - 2*a0*aMax + 2*af*aMax + 2*jMax*(aMax*tf + v0 - vf))); + profile.t[4] = -(4*Power(a0,3)*jMax - 6*a0*Power(af,2)*jMax + 2*Power(af,3)*jMax - 12*Power(a0,2)*aMax*jMax + 12*a0*af*aMax*jMax + 6*a0*Power(aMax,2)*jMax - 6*af*Power(aMax,2)*jMax + 12*Power(jMax,3)*p0 - 12*Power(jMax,3)*pf + 6*Power(af,2)*Power(jMax,2)*tf + 12*a0*aMax*Power(jMax,2)*tf - 12*af*aMax*Power(jMax,2)*tf - 6*Power(aMax,2)*Power(jMax,2)*tf - 6*aMax*Power(jMax,3)*Power(tf,2) + 12*a0*Power(jMax,2)*v0 - 12*aMax*Power(jMax,2)*v0 - 12*a0*Power(jMax,2)*vf + 12*aMax*Power(jMax,2)*vf + 12*Power(jMax,3)*tf*vf + h1)/h2; + profile.t[5] = (Power(a0,3) - Power(af,3) + 3*Power(a0,2)*(af - 2*aMax + jMax*tf) + 3*Power(af,2)*(2*aMax + jMax*tf) - 3*a0*(Power(af,2) - 2*(Power(aMax,2) + jMax*(v0 - vf))) - 6*af*(Power(aMax,2) + jMax*(-v0 + vf)) + 6*jMax*(-(aMax*(aMax*tf + 2*v0 - 2*vf)) + jMax*(2*p0 - 2*pf + tf*(v0 + vf))))/(3.*jMax*(Power(a0,2) - Power(af,2) - 2*a0*aMax + 2*af*aMax + 2*jMax*(aMax*tf + v0 - vf))); + profile.t[6] = (-af + aMax)/jMax; profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, jMax, 0, -jMax}); if (profile.check(tf, pf, vf, af, vMax, aMax)) { @@ -425,12 +412,15 @@ bool RuckigStep2::time_up_acc1(Profile& profile, double vMax, double aMax, doubl bool RuckigStep2::time_up_acc0(Profile& profile, double vMax, double aMax, double jMax) { // a3 != 0 + double h1 = Sqrt(2)*Sqrt(Power(jMax,2)*(2*Power(Power(a0,3) + 2*Power(af,3) - 6*Power(af,2)*aMax + 9*af*Power(aMax,2) - 6*Power(aMax,3) - 6*af*aMax*jMax*tf + 9*Power(aMax,2)*jMax*tf + 3*a0*aMax*(-2*af + 3*aMax - 2*jMax*tf) + 3*Power(a0,2)*(af - 2*aMax + jMax*tf) - 6*Power(jMax,2)*(p0 - pf + tf*v0) + 6*af*jMax*(-v0 + vf) - 3*aMax*jMax*(jMax*Power(tf,2) - 2*v0 + 2*vf),2) - 9*Power(Power(a0,2) + Power(af,2) - 2*a0*aMax - 2*af*aMax + 2*(Power(aMax,2) - aMax*jMax*tf - jMax*v0 + jMax*vf),3))); + double h2 = (6.*Power(jMax,2)*(Power(a0,2) + Power(af,2) - 2*a0*aMax - 2*af*aMax + 2*(Power(aMax,2) - aMax*jMax*tf - jMax*v0 + jMax*vf))); + // Solution 1 profile.t[0] = (-a0 + aMax)/jMax; - profile.t[1] = -(4*Power(a0,3)*jMax - 12*Power(a0,2)*aMax*jMax + 18*a0*Power(aMax,2)*jMax - 12*Power(aMax,3)*jMax + 12*Power(jMax,3)*p0 - 12*Power(jMax,3)*pf - 12*a0*aMax*Power(jMax,2)*tf + 18*Power(aMax,2)*Power(jMax,2)*tf - 6*aMax*Power(jMax,3)*Power(tf,2) - 12*a0*Power(jMax,2)*v0 + 12*aMax*Power(jMax,2)*v0 + 12*a0*Power(jMax,2)*vf - 12*aMax*Power(jMax,2)*vf + 12*Power(jMax,3)*tf*vf + Sqrt(2)*Sqrt(Power(jMax,2)*(2*Power(Power(a0,3) - 6*Power(aMax,3) + 9*Power(aMax,2)*jMax*tf + 3*a0*aMax*(3*aMax - 2*jMax*tf) + Power(a0,2)*(-6*aMax + 3*jMax*tf) - 6*Power(jMax,2)*(p0 - pf + tf*v0) - 3*aMax*jMax*(jMax*Power(tf,2) - 2*v0 + 2*vf),2) - 9*Power(Power(a0,2) - 2*a0*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*(-v0 + vf)),3))))/(6.*Power(jMax,2)*(-Power(a0,2) + 2*a0*aMax - 2*Power(aMax,2) + 2*aMax*jMax*tf + 2*jMax*v0 - 2*jMax*vf)); - profile.t[2] = (2*Power(a0,3)*jMax - 12*Power(a0,2)*aMax*jMax + 18*a0*Power(aMax,2)*jMax - 12*Power(aMax,3)*jMax - 12*Power(jMax,3)*p0 + 12*Power(jMax,3)*pf + 6*Power(a0,2)*Power(jMax,2)*tf - 12*a0*aMax*Power(jMax,2)*tf + 18*Power(aMax,2)*Power(jMax,2)*tf - 6*aMax*Power(jMax,3)*Power(tf,2) + 12*aMax*Power(jMax,2)*v0 - 12*Power(jMax,3)*tf*v0 - 12*aMax*Power(jMax,2)*vf + Sqrt(2)*Sqrt(Power(jMax,2)*(2*Power(Power(a0,3) - 6*Power(aMax,3) + 9*Power(aMax,2)*jMax*tf + 3*a0*aMax*(3*aMax - 2*jMax*tf) + Power(a0,2)*(-6*aMax + 3*jMax*tf) - 6*Power(jMax,2)*(p0 - pf + tf*v0) - 3*aMax*jMax*(jMax*Power(tf,2) - 2*v0 + 2*vf),2) - 9*Power(Power(a0,2) - 2*a0*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*(-v0 + vf)),3))))/(6.*Power(jMax,2)*(-Power(a0,2) + 2*a0*aMax - 2*Power(aMax,2) + 2*aMax*jMax*tf + 2*jMax*v0 - 2*jMax*vf)); - profile.t[3] = (-2*Power(a0,3)*jMax + 12*Power(a0,2)*aMax*jMax - 18*a0*Power(aMax,2)*jMax + 12*Power(aMax,3)*jMax + 12*Power(jMax,3)*p0 - 12*Power(jMax,3)*pf - 6*Power(a0,2)*Power(jMax,2)*tf + 12*a0*aMax*Power(jMax,2)*tf - 18*Power(aMax,2)*Power(jMax,2)*tf + 6*aMax*Power(jMax,3)*Power(tf,2) - 12*aMax*Power(jMax,2)*v0 + 12*Power(jMax,3)*tf*v0 + 12*aMax*Power(jMax,2)*vf + Sqrt(2)*Sqrt(Power(jMax,2)*(2*Power(Power(a0,3) - 6*Power(aMax,3) + 9*Power(aMax,2)*jMax*tf + 3*a0*aMax*(3*aMax - 2*jMax*tf) + Power(a0,2)*(-6*aMax + 3*jMax*tf) - 6*Power(jMax,2)*(p0 - pf + tf*v0) - 3*aMax*jMax*(jMax*Power(tf,2) - 2*v0 + 2*vf),2) - 9*Power(Power(a0,2) - 2*a0*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*(-v0 + vf)),3))))/(6.*Power(jMax,2)*(-Power(a0,2) + 2*a0*aMax - 2*Power(aMax,2) + 2*aMax*jMax*tf + 2*jMax*v0 - 2*jMax*vf)); - profile.t[4] = -(2*Power(a0,3)*jMax - 6*Power(a0,2)*aMax*jMax + 6*a0*Power(aMax,2)*jMax - 12*Power(jMax,3)*p0 + 12*Power(jMax,3)*pf + 6*Power(a0,2)*Power(jMax,2)*tf - 12*a0*aMax*Power(jMax,2)*tf + 6*Power(aMax,2)*Power(jMax,2)*tf - 6*aMax*Power(jMax,3)*Power(tf,2) - 12*Power(jMax,3)*tf*v0 + Sqrt(2)*Sqrt(Power(jMax,2)*(2*Power(Power(a0,3) - 6*Power(aMax,3) + 9*Power(aMax,2)*jMax*tf + 3*a0*aMax*(3*aMax - 2*jMax*tf) + Power(a0,2)*(-6*aMax + 3*jMax*tf) - 6*Power(jMax,2)*(p0 - pf + tf*v0) - 3*aMax*jMax*(jMax*Power(tf,2) - 2*v0 + 2*vf),2) - 9*Power(Power(a0,2) - 2*a0*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*(-v0 + vf)),3))))/(6.*Power(jMax,2)*(-Power(a0,2) + 2*a0*aMax - 2*Power(aMax,2) + 2*aMax*jMax*tf + 2*jMax*v0 - 2*jMax*vf)); + profile.t[1] = -(-4*Power(a0,3)*jMax - 6*a0*Power(af,2)*jMax - 2*Power(af,3)*jMax + 12*Power(a0,2)*aMax*jMax + 12*a0*af*aMax*jMax + 12*Power(af,2)*aMax*jMax - 18*a0*Power(aMax,2)*jMax - 18*af*Power(aMax,2)*jMax + 12*Power(aMax,3)*jMax - 12*Power(jMax,3)*p0 + 12*Power(jMax,3)*pf - 6*Power(af,2)*Power(jMax,2)*tf + 12*a0*aMax*Power(jMax,2)*tf + 12*af*aMax*Power(jMax,2)*tf - 18*Power(aMax,2)*Power(jMax,2)*tf + 6*aMax*Power(jMax,3)*Power(tf,2) + 12*a0*Power(jMax,2)*v0 - 12*aMax*Power(jMax,2)*v0 - 12*a0*Power(jMax,2)*vf + 12*aMax*Power(jMax,2)*vf - 12*Power(jMax,3)*tf*vf - h1)/h2; + profile.t[2] = -(2*Power(a0,3)*jMax + 6*Power(a0,2)*af*jMax + 4*Power(af,3)*jMax - 12*Power(a0,2)*aMax*jMax - 12*a0*af*aMax*jMax - 12*Power(af,2)*aMax*jMax + 18*a0*Power(aMax,2)*jMax + 18*af*Power(aMax,2)*jMax - 12*Power(aMax,3)*jMax - 12*Power(jMax,3)*p0 + 12*Power(jMax,3)*pf + 6*Power(a0,2)*Power(jMax,2)*tf - 12*a0*aMax*Power(jMax,2)*tf - 12*af*aMax*Power(jMax,2)*tf + 18*Power(aMax,2)*Power(jMax,2)*tf - 6*aMax*Power(jMax,3)*Power(tf,2) - 12*af*Power(jMax,2)*v0 + 12*aMax*Power(jMax,2)*v0 - 12*Power(jMax,3)*tf*v0 + 12*af*Power(jMax,2)*vf - 12*aMax*Power(jMax,2)*vf + h1)/h2; + profile.t[3] = -(-2*Power(a0,3)*jMax - 6*Power(a0,2)*af*jMax - 4*Power(af,3)*jMax + 12*Power(a0,2)*aMax*jMax + 12*a0*af*aMax*jMax + 12*Power(af,2)*aMax*jMax - 18*a0*Power(aMax,2)*jMax - 18*af*Power(aMax,2)*jMax + 12*Power(aMax,3)*jMax + 12*Power(jMax,3)*p0 - 12*Power(jMax,3)*pf - 6*Power(a0,2)*Power(jMax,2)*tf + 12*a0*aMax*Power(jMax,2)*tf + 12*af*aMax*Power(jMax,2)*tf - 18*Power(aMax,2)*Power(jMax,2)*tf + 6*aMax*Power(jMax,3)*Power(tf,2) + 12*af*Power(jMax,2)*v0 - 12*aMax*Power(jMax,2)*v0 + 12*Power(jMax,3)*tf*v0 - 12*af*Power(jMax,2)*vf + 12*aMax*Power(jMax,2)*vf + h1)/h2; + profile.t[4] = -(-2*Power(a0,3)*jMax + 2*Power(af,3)*jMax + 6*Power(a0,2)*aMax*jMax - 6*Power(af,2)*aMax*jMax - 6*a0*Power(aMax,2)*jMax + 6*af*Power(aMax,2)*jMax + 12*Power(jMax,3)*p0 - 12*Power(jMax,3)*pf - 6*Power(a0,2)*Power(jMax,2)*tf + 12*a0*aMax*Power(jMax,2)*tf - 6*Power(aMax,2)*Power(jMax,2)*tf + 6*aMax*Power(jMax,3)*Power(tf,2) + 12*Power(jMax,3)*tf*v0 - h1)/h2; profile.t[5] = 0; profile.t[6] = 0;