diff --git a/include/ruckig/ruckig.hpp b/include/ruckig/ruckig.hpp index e7a51d74..496e1640 100644 --- a/include/ruckig/ruckig.hpp +++ b/include/ruckig/ruckig.hpp @@ -115,8 +115,9 @@ class Ruckig { return false; } - if ((input.target_acceleration.array() != 0.0).any()) { - std::cerr << "Ruckig does not support a target acceleration." << std::endl; + auto max_target_acceleration = (2 * input.max_jerk.array() * (input.max_velocity.array() - input.target_velocity.array().abs())).sqrt(); + if ((input.target_acceleration.array().abs() > max_target_acceleration.array()).any()) { + std::cerr << "Target acceleration exceeds maximal possible acceleration." << std::endl; return false; } diff --git a/notebooks/ruckig-step1.nb b/notebooks/ruckig-step1.nb index 6cd704c1..b77be412 100644 --- a/notebooks/ruckig-step1.nb +++ b/notebooks/ruckig-step1.nb @@ -10,10 +10,10 @@ NotebookFileLineBreakTest NotebookFileLineBreakTest NotebookDataPosition[ 158, 7] -NotebookDataLength[ 26807, 739] -NotebookOptionsPosition[ 25312, 708] -NotebookOutlinePosition[ 25705, 724] -CellTagsIndexPosition[ 25662, 721] +NotebookDataLength[ 25905, 701] +NotebookOptionsPosition[ 24409, 670] +NotebookOutlinePosition[ 24803, 686] +CellTagsIndexPosition[ 24760, 683] WindowFrame->Normal*) (* Beginning of Notebook Content *) @@ -29,16 +29,23 @@ http://ais.uni-bonn.de/papers/ICUAS_2016_Beul.pdf\ 2db88daacc31"], Cell[BoxData[{ + RowBox[{ + RowBox[{"jerkProfileUDDU", "=", + RowBox[{"{", + RowBox[{"jMax", ",", "0", ",", + RowBox[{"-", "jMax"}], ",", "0", ",", + RowBox[{"-", "jMax"}], ",", "0", ",", "jMax"}], "}"}]}], + ";"}], "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"jerkProfileUDDU", "=", + RowBox[{"jerkProfileUDUD", "=", RowBox[{"{", RowBox[{"jMax", ",", "0", ",", - RowBox[{"-", "jMax"}], ",", "0", ",", - RowBox[{"-", "jMax"}], ",", "0", ",", "jMax"}], "}"}]}], ";"}], + RowBox[{"-", "jMax"}], ",", "0", ",", "jMax", ",", "0", ",", + RowBox[{"-", "jMax"}]}], "}"}]}], ";"}], "\[IndentingNewLine]"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"jerkProfile", "=", "jerkProfileUDDU"}], + RowBox[{"jerkProfile", "=", "jerkProfileUDUD"}], ";"}], "\[IndentingNewLine]", RowBox[{ RowBox[{ @@ -240,9 +247,11 @@ Cell[BoxData[{ SuperscriptBox["t7", "3"], RowBox[{"jerkProfile", "[", RowBox[{"[", "7", "]"}], "]"}]}]}]}], ";"}]}], "Input", - CellChangeTimes->{{3.817462841378334*^9, 3.817462900269423*^9}}, + CellChangeTimes->{{3.817462841378334*^9, 3.817462900269423*^9}, { + 3.8176136118348*^9, 3.8176136203885107`*^9}, 3.817613860612129*^9, + 3.817615902502871*^9}, CellLabel-> - "In[872]:=",ExpressionUUID->"d346b209-9dd3-47cf-8f1e-f04bc3bf980a"], + "In[1141]:=",ExpressionUUID->"d346b209-9dd3-47cf-8f1e-f04bc3bf980a"], Cell[BoxData[ RowBox[{ @@ -357,9 +366,10 @@ Cell[BoxData[ "\[IndentingNewLine]", "}"}]}], ";"}]], "Input", CellChangeTimes->{{3.8174629257969522`*^9, 3.817462978356163*^9}, { 3.817463017710661*^9, 3.817463019150318*^9}, {3.817463125060719*^9, - 3.817463268801898*^9}, {3.8174633497876797`*^9, 3.817463350013275*^9}}, + 3.817463268801898*^9}, {3.8174633497876797`*^9, 3.817463350013275*^9}, { + 3.8176142815731697`*^9, 3.817614282974786*^9}}, CellLabel-> - "In[896]:=",ExpressionUUID->"ccba75f7-892e-4119-93ec-41811b2e330e"], + "In[1303]:=",ExpressionUUID->"ccba75f7-892e-4119-93ec-41811b2e330e"], Cell["\<\ Information Roots are max. 4th Order @@ -370,13 +380,14 @@ Information Roots are max. 4th Order - Case 5: Vel: Solution 3/4 - Case 6: Acc1: Root in t1 - Case 7: Acc0: Root in t5 -- Case 8: None: Root in t1\ +- Case 8: None: Root in t1, (UDDU), (UDUD for af != 0)\ \>", "Text", CellChangeTimes->{{3.8174632785384893`*^9, 3.817463379445586*^9}, { 3.8174635726156178`*^9, 3.817463721259947*^9}, {3.8174665706088743`*^9, 3.817466570913156*^9}, {3.817467118701497*^9, 3.8174671190303392`*^9}, { - 3.8174674657907257`*^9, - 3.8174674659023438`*^9}},ExpressionUUID->"e82cb2e9-b4f4-4bc2-9a74-\ + 3.8174674657907257`*^9, 3.8174674659023438`*^9}, {3.817616051327691*^9, + 3.817616061734858*^9}, {3.8176181077496033`*^9, + 3.817618117117276*^9}},ExpressionUUID->"e82cb2e9-b4f4-4bc2-9a74-\ 547157d3c4dc"], Cell["\<\ @@ -505,12 +516,12 @@ Cell[CellGroupData[{ Cell[BoxData[{ RowBox[{ - RowBox[{"tRoot", "=", "1"}], ";"}], "\[IndentingNewLine]", + RowBox[{"tRoot", "=", "5"}], ";"}], "\[IndentingNewLine]", RowBox[{ RowBox[{"tmpResult", "=", RowBox[{"resultT", "[", RowBox[{"[", - RowBox[{"8", ",", "1"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"7", ",", "1"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ RowBox[{"rootFunction", "=", RowBox[{"tmpResult", "[", @@ -528,14 +539,13 @@ Cell[BoxData[{ RowBox[{"[", "1", "]"}], "]"}], "[", "x", "]"}], ",", "x"}], "]"}], "[", RowBox[{"[", "1", "]"}], "]"}], - RowBox[{ + RowBox[{"Last", "[", RowBox[{"CoefficientList", "[", RowBox[{ RowBox[{ RowBox[{"rootFunction", "[", RowBox[{"[", "1", "]"}], "]"}], "[", "x", "]"}], ",", "x"}], "]"}], - "[", - RowBox[{"[", "5", "]"}], "]"}]], "]"}]}], "\[IndentingNewLine]", + "]"}]], "]"}]}], "\[IndentingNewLine]", RowBox[{ RowBox[{"ToString", "[", RowBox[{"tmp", ",", "CForm"}], "]"}], ";"}], "\[IndentingNewLine]", @@ -547,113 +557,58 @@ Cell[BoxData[{ 3.817464978456039*^9}, {3.817465392157484*^9, 3.8174654363653917`*^9}, { 3.8174655875222387`*^9, 3.817465641579135*^9}, {3.8174662104318943`*^9, 3.817466215391354*^9}, {3.817466254017514*^9, 3.817466312191326*^9}, - 3.817473469655705*^9}, + 3.817473469655705*^9, {3.817616070193791*^9, 3.8176160704001007`*^9}, { + 3.817616106402553*^9, 3.817616138705028*^9}, {3.81761645464653*^9, + 3.817616521373534*^9}, {3.817618123606201*^9, 3.817618152793016*^9}}, CellLabel-> - "In[921]:=",ExpressionUUID->"ca8158ca-2cc6-4c2b-92b0-ae9728cb14cd"], + "In[1333]:=",ExpressionUUID->"ca8158ca-2cc6-4c2b-92b0-ae9728cb14cd"], Cell[BoxData[ - RowBox[{"-", + RowBox[{ + FractionBox["1", + RowBox[{"12", " ", + SuperscriptBox["jMax", "4"]}]], RowBox[{"(", RowBox[{ - RowBox[{"(", - RowBox[{ - SuperscriptBox["a0", "6"], "+", - SuperscriptBox["af", "6"], "+", - RowBox[{"48", " ", - SuperscriptBox["af", "3"], " ", - SuperscriptBox["jMax", "2"], " ", - RowBox[{"(", - RowBox[{"p0", "-", "pf"}], ")"}]}], "-", - RowBox[{"144", " ", "af", " ", - SuperscriptBox["jMax", "3"], " ", - RowBox[{"(", - RowBox[{"p0", "-", "pf"}], ")"}], " ", "vf"}], "-", - RowBox[{"6", " ", - SuperscriptBox["af", "4"], " ", "jMax", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"3", " ", "v0"}], "+", "vf"}], ")"}]}], "+", - RowBox[{"16", " ", - SuperscriptBox["a0", "3"], " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["af", "3"], "+", - RowBox[{"3", " ", - SuperscriptBox["jMax", "2"], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"-", "p0"}], "+", "pf"}], ")"}]}], "-", - RowBox[{"3", " ", "af", " ", "jMax", " ", "vf"}]}], ")"}]}], "+", - RowBox[{"48", " ", "a0", " ", "jMax", " ", "v0", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["af", "3"], "+", - RowBox[{"3", " ", - SuperscriptBox["jMax", "2"], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"-", "p0"}], "+", "pf"}], ")"}]}], "-", - RowBox[{"3", " ", "af", " ", "jMax", " ", "vf"}]}], ")"}]}], "-", - RowBox[{"36", " ", - SuperscriptBox["af", "2"], " ", - SuperscriptBox["jMax", "2"], " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["v0", "2"], "-", - RowBox[{"2", " ", "v0", " ", "vf"}], "-", - SuperscriptBox["vf", "2"]}], ")"}]}], "-", - RowBox[{"72", " ", - SuperscriptBox["jMax", "3"], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"jMax", " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{"p0", "-", "pf"}], ")"}], "2"]}], "-", - RowBox[{ - RowBox[{"(", - RowBox[{"v0", "-", "vf"}], ")"}], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{"v0", "+", "vf"}], ")"}], "2"]}]}], ")"}]}], "+", + RowBox[{ + RowBox[{"-", "3"}], " ", + SuperscriptBox["a0", "4"]}], "+", + RowBox[{"3", " ", + SuperscriptBox["af", "4"]}], "+", + RowBox[{"8", " ", + SuperscriptBox["a0", "3"], " ", "aMax"}], "-", + RowBox[{"8", " ", + SuperscriptBox["af", "3"], " ", "aMax"}], "-", + RowBox[{"24", " ", "a0", " ", "aMax", " ", "jMax", " ", "v0"}], "-", + RowBox[{"6", " ", + SuperscriptBox["a0", "2"], " ", + RowBox[{"(", RowBox[{ - SuperscriptBox["a0", "4"], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "9"}], " ", - SuperscriptBox["af", "2"]}], "+", - RowBox[{"6", " ", "jMax", " ", - RowBox[{"(", - RowBox[{"v0", "+", - RowBox[{"3", " ", "vf"}]}], ")"}]}]}], ")"}]}], "-", - RowBox[{"9", " ", - SuperscriptBox["a0", "2"], " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["af", "4"], "+", - RowBox[{"4", " ", - SuperscriptBox["af", "2"], " ", "jMax", " ", - RowBox[{"(", - RowBox[{"v0", "-", "vf"}], ")"}]}], "-", - RowBox[{"4", " ", - SuperscriptBox["jMax", "2"], " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["v0", "2"], "+", - RowBox[{"2", " ", "v0", " ", "vf"}], "-", - SuperscriptBox["vf", "2"]}], ")"}]}]}], ")"}]}]}], ")"}], "/", - RowBox[{"(", - RowBox[{"36", " ", - SuperscriptBox["jMax", "4"], " ", - RowBox[{"(", + SuperscriptBox["aMax", "2"], "-", + RowBox[{"2", " ", "jMax", " ", "v0"}]}], ")"}]}], "-", + RowBox[{"24", " ", "af", " ", "aMax", " ", "jMax", " ", "vf"}], "+", + RowBox[{"6", " ", + SuperscriptBox["af", "2"], " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["aMax", "2"], "+", + RowBox[{"2", " ", "jMax", " ", "vf"}]}], ")"}]}], "+", + RowBox[{"12", " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "aMax", " ", "jMax", " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf"}], ")"}]}], "+", RowBox[{ - RowBox[{"-", - SuperscriptBox["a0", "2"]}], "+", - SuperscriptBox["af", "2"], "+", - RowBox[{"2", " ", "jMax", " ", - RowBox[{"(", - RowBox[{"v0", "-", "vf"}], ")"}]}]}], ")"}]}], ")"}]}], + SuperscriptBox["aMax", "2"], " ", + RowBox[{"(", + RowBox[{"v0", "+", "vf"}], ")"}]}], "+", + RowBox[{"jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + SuperscriptBox["v0", "2"]}], "+", + SuperscriptBox["vf", "2"]}], ")"}]}]}], ")"}]}]}], ")"}]}]], "Output", CellChangeTimes->{ 3.817464224080229*^9, {3.8174646757284613`*^9, 3.817464735056591*^9}, { @@ -661,9 +616,11 @@ Cell[BoxData[ 3.817464979046924*^9}, {3.8174653964553947`*^9, 3.81746543673561*^9}, { 3.817465591806774*^9, 3.817465642153749*^9}, 3.817466216455537*^9, { 3.817466257823419*^9, 3.817466313043701*^9}, {3.8174734582456903`*^9, - 3.8174734702907677`*^9}}, + 3.8174734702907677`*^9}, {3.817616067838916*^9, 3.817616070838888*^9}, { + 3.817616106894815*^9, 3.817616139189769*^9}, {3.817616459558393*^9, + 3.817616521866693*^9}, {3.817618128281868*^9, 3.817618153255725*^9}}, CellLabel-> - "Out[924]=",ExpressionUUID->"5b170d15-b67c-4399-b6a6-2cb831cc6258"] + "Out[1336]=",ExpressionUUID->"5041e5a2-bdd6-4cbc-947c-5a048a6fee8c"] }, Open ]], Cell[CellGroupData[{ @@ -676,10 +633,10 @@ Cell[BoxData[{ RowBox[{"rootFunction", "\[Rule]", "t"}], "}"}]}]}], ";"}], "\[IndentingNewLine]", RowBox[{"tmp", "=", - RowBox[{"FullSimplify", "[", + RowBox[{"Simplify", "[", RowBox[{"tmpReplaced", "[", RowBox[{"[", - RowBox[{"1", ",", "2"}], "]"}], "]"}], "]"}]}], "\[IndentingNewLine]", + RowBox[{"7", ",", "2"}], "]"}], "]"}], "]"}]}], "\[IndentingNewLine]", RowBox[{ RowBox[{"ToString", "[", RowBox[{"tmp", ",", "CForm"}], "]"}], ";"}], "\[IndentingNewLine]", @@ -690,24 +647,29 @@ Cell[BoxData[{ 3.817465468359084*^9, 3.817465497350111*^9}, {3.8174656551333303`*^9, 3.817465701685096*^9}, {3.817473479559062*^9, 3.8174734800856733`*^9}, { 3.817473533672271*^9, 3.817473533751308*^9}, {3.8174742089250727`*^9, - 3.817474210971596*^9}}, + 3.817474210971596*^9}, {3.817616151988146*^9, 3.817616183627185*^9}, { + 3.817618162951096*^9, 3.8176181782700644`*^9}}, CellLabel-> - "In[974]:=",ExpressionUUID->"02ddacce-ea40-40b5-bbc9-e4e0e8a8500b"], + "In[1363]:=",ExpressionUUID->"02ddacce-ea40-40b5-bbc9-e4e0e8a8500b"], -Cell[BoxData["t"], "Output", +Cell[BoxData[ + RowBox[{ + RowBox[{"-", + FractionBox["af", "jMax"]}], "+", "t"}]], "Output", CellChangeTimes->{{3.817464289219615*^9, 3.817464337401743*^9}, { 3.817464881225731*^9, 3.817464920476918*^9}, {3.817464988238978*^9, 3.81746500768358*^9}, {3.81746540086765*^9, 3.817465413431589*^9}, { 3.817465461523279*^9, 3.817465497766103*^9}, {3.817465655693458*^9, 3.817465703495722*^9}, 3.817466232751576*^9, 3.817473410326756*^9, { 3.817473459931615*^9, 3.8174734881471148`*^9}, 3.81747354157975*^9, { - 3.81747420945721*^9, 3.8174742112747707`*^9}}, + 3.81747420945721*^9, 3.8174742112747707`*^9}, {3.8176161501097183`*^9, + 3.817616185177759*^9}, {3.817618163459977*^9, 3.817618178516623*^9}}, CellLabel-> - "Out[975]=",ExpressionUUID->"2b4298b3-1aff-4d08-ad86-f0cf06416c89"] + "Out[1364]=",ExpressionUUID->"b12da6aa-4361-4dff-b279-45c07bfe4387"] }, Open ]] }, WindowSize->{872, 937}, -WindowMargins->{{55, Automatic}, {Automatic, 21}}, +WindowMargins->{{Automatic, 310}, {Automatic, 29}}, FrontEndVersion->"12.1 for Mac OS X x86 (64-bit) (June 19, 2020)", StyleDefinitions->"Default.nb", ExpressionUUID->"c8b287f8-8b4a-4e98-ab1e-df7dcd999be5" @@ -724,22 +686,22 @@ CellTagsIndex->{} (*NotebookFileOutline Notebook[{ Cell[558, 20, 250, 8, 81, "Text",ExpressionUUID->"f0efc888-777d-4c66-b8e6-2db88daacc31"], -Cell[811, 30, 6675, 214, 854, "Input",ExpressionUUID->"d346b209-9dd3-47cf-8f1e-f04bc3bf980a"], -Cell[7489, 246, 4733, 115, 283, "Input",ExpressionUUID->"ccba75f7-892e-4119-93ec-41811b2e330e"], -Cell[12225, 363, 632, 16, 219, "Text",ExpressionUUID->"e82cb2e9-b4f4-4bc2-9a74-547157d3c4dc"], -Cell[12860, 381, 249, 7, 58, "Text",ExpressionUUID->"2bb13fa1-f9ef-4a5d-ac3b-101e020ad421"], +Cell[811, 30, 7012, 223, 875, "Input",ExpressionUUID->"d346b209-9dd3-47cf-8f1e-f04bc3bf980a"], +Cell[7826, 255, 4785, 116, 283, "Input",ExpressionUUID->"ccba75f7-892e-4119-93ec-41811b2e330e"], +Cell[12614, 373, 757, 17, 219, "Text",ExpressionUUID->"e82cb2e9-b4f4-4bc2-9a74-547157d3c4dc"], +Cell[13374, 392, 249, 7, 58, "Text",ExpressionUUID->"2bb13fa1-f9ef-4a5d-ac3b-101e020ad421"], Cell[CellGroupData[{ -Cell[13134, 392, 1202, 26, 73, "Input",ExpressionUUID->"f20278fb-5b15-4c9c-bd38-9f1d3931e2f3"], -Cell[14339, 420, 3149, 75, 126, "Output",ExpressionUUID->"7426b662-e9ba-4085-9103-9ba45f37977b"] +Cell[13648, 403, 1202, 26, 73, "Input",ExpressionUUID->"f20278fb-5b15-4c9c-bd38-9f1d3931e2f3"], +Cell[14853, 431, 3149, 75, 126, "Output",ExpressionUUID->"7426b662-e9ba-4085-9103-9ba45f37977b"] }, Open ]], -Cell[17503, 498, 166, 3, 35, "Text",ExpressionUUID->"8d7af2fe-a47e-4ef4-b28a-db0dc764dd0f"], +Cell[18017, 509, 166, 3, 35, "Text",ExpressionUUID->"8d7af2fe-a47e-4ef4-b28a-db0dc764dd0f"], Cell[CellGroupData[{ -Cell[17694, 505, 1806, 46, 159, "Input",ExpressionUUID->"ca8158ca-2cc6-4c2b-92b0-ae9728cb14cd"], -Cell[19503, 553, 4071, 112, 129, "Output",ExpressionUUID->"5b170d15-b67c-4399-b6a6-2cb831cc6258"] +Cell[18208, 516, 1977, 47, 159, "Input",ExpressionUUID->"ca8158ca-2cc6-4c2b-92b0-ae9728cb14cd"], +Cell[20188, 565, 2224, 57, 102, "Output",ExpressionUUID->"5041e5a2-bdd6-4cbc-947c-5a048a6fee8c"] }, Open ]], Cell[CellGroupData[{ -Cell[23611, 670, 1093, 24, 94, "Input",ExpressionUUID->"02ddacce-ea40-40b5-bbc9-e4e0e8a8500b"], -Cell[24707, 696, 589, 9, 57, "Output",ExpressionUUID->"2b4298b3-1aff-4d08-ad86-f0cf06416c89"] +Cell[22449, 627, 1187, 25, 94, "Input",ExpressionUUID->"02ddacce-ea40-40b5-bbc9-e4e0e8a8500b"], +Cell[23639, 654, 754, 13, 54, "Output",ExpressionUUID->"b12da6aa-4361-4dff-b279-45c07bfe4387"] }, Open ]] } ] diff --git a/notebooks/ruckig-step2.nb b/notebooks/ruckig-step2.nb index 4d350d5d..eb4e5f30 100644 --- a/notebooks/ruckig-step2.nb +++ b/notebooks/ruckig-step2.nb @@ -10,10 +10,10 @@ NotebookFileLineBreakTest NotebookFileLineBreakTest NotebookDataPosition[ 158, 7] -NotebookDataLength[ 59800, 1547] -NotebookOptionsPosition[ 56755, 1491] -NotebookOutlinePosition[ 57177, 1508] -CellTagsIndexPosition[ 57134, 1505] +NotebookDataLength[ 103220, 2710] +NotebookOptionsPosition[ 99842, 2649] +NotebookOutlinePosition[ 100264, 2666] +CellTagsIndexPosition[ 100221, 2663] WindowFrame->Normal*) (* Beginning of Notebook Content *) @@ -257,9 +257,11 @@ Cell[BoxData[{ 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.8175564232104387`*^9, - 3.817556749971588*^9}, + 3.817556749971588*^9, {3.817559034590983*^9, 3.817559035171609*^9}, + 3.817575769503861*^9, 3.817577037096459*^9, 3.8175774841034517`*^9, + 3.817577983155306*^9, 3.817610819742194*^9}, CellLabel-> - "In[179]:=",ExpressionUUID->"9382707b-2876-460d-b0fb-ca70e05b1e9a"], + "In[813]:=",ExpressionUUID->"9382707b-2876-460d-b0fb-ca70e05b1e9a"], Cell[BoxData[ RowBox[{ @@ -434,7 +436,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"}], ",", @@ -601,9 +603,10 @@ Cell[BoxData[ 3.817491313811269*^9, 3.81749131393857*^9}, {3.817539498083053*^9, 3.817539499801177*^9}, {3.817539754038406*^9, 3.817539754218478*^9}, { 3.817556107819336*^9, 3.8175561104785957`*^9}, {3.817556816140629*^9, - 3.817556833804111*^9}}, + 3.817556833804111*^9}, {3.817560685985124*^9, 3.817560696143289*^9}, { + 3.817560749066228*^9, 3.81756075437642*^9}}, CellLabel-> - "In[206]:=",ExpressionUUID->"93a49805-7b39-4e71-a5ce-78f2b77ae9c0"], + "In[839]:=",ExpressionUUID->"93a49805-7b39-4e71-a5ce-78f2b77ae9c0"], Cell["\<\ Information @@ -828,7 +831,7 @@ Cell[BoxData[{ RowBox[{"tmpResult", "=", RowBox[{"resultT", "[", RowBox[{"[", - RowBox[{"6", ",", "1"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"8", ",", "1"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ RowBox[{"rootFunction", "=", RowBox[{"tmpResult", "[", @@ -862,72 +865,130 @@ Cell[BoxData[{ 3.817491349584128*^9}, {3.817491538900215*^9, 3.817491568052608*^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}}, + 3.817556590540579*^9, 3.8175566069951468`*^9}, {3.81761051905612*^9, + 3.817610526067877*^9}, {3.8176106267544737`*^9, 3.8176106635777063`*^9}, { + 3.817610854495672*^9, 3.817610865358349*^9}, {3.817610921121066*^9, + 3.817610939287958*^9}}, CellLabel-> - "In[145]:=",ExpressionUUID->"217d7885-1018-483e-8952-08082feb5695"], + "In[876]:=",ExpressionUUID->"217d7885-1018-483e-8952-08082feb5695"], Cell[BoxData[ RowBox[{ FractionBox["1", - RowBox[{"12", " ", - SuperscriptBox["jMax", "4"]}]], + RowBox[{"144", " ", + SuperscriptBox["jMax", "5"], " ", + RowBox[{"(", + RowBox[{"a0", "-", "af", "+", + RowBox[{"jMax", " ", "tf"}]}], ")"}]}]], 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[{"7", " ", + SuperscriptBox["a0", "6"]}], "+", + SuperscriptBox["af", "6"], "-", + RowBox[{"12", " ", + SuperscriptBox["a0", "5"], " ", RowBox[{"(", - RowBox[{"v0", "-", "vf"}], ")"}]}], "-", - RowBox[{"6", " ", - SuperscriptBox["a0", "2"], " ", + RowBox[{"af", "-", + RowBox[{"jMax", " ", "tf"}]}], ")"}]}], "+", + RowBox[{"48", " ", + SuperscriptBox["af", "3"], " ", + SuperscriptBox["jMax", "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[{"p0", "-", "pf", "+", + RowBox[{"tf", " ", "v0"}]}], ")"}]}], "-", + RowBox[{"8", " ", + SuperscriptBox["a0", "3"], " ", RowBox[{"(", RowBox[{ - RowBox[{"2", " ", "aMax", " ", "jMax", " ", + SuperscriptBox["af", "3"], "-", + RowBox[{"3", " ", + SuperscriptBox["jMax", "2"], " ", RowBox[{"(", RowBox[{"p0", "-", "pf", "+", RowBox[{"tf", " ", "v0"}]}], ")"}]}], "+", + RowBox[{"3", " ", "af", " ", "jMax", " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}]}]}], ")"}]}], "-", + RowBox[{"72", " ", + SuperscriptBox["jMax", "3"], " ", + RowBox[{"(", + RowBox[{ RowBox[{"jMax", " ", SuperscriptBox[ RowBox[{"(", - RowBox[{"v0", "-", "vf"}], ")"}], "2"]}], "+", - RowBox[{ - SuperscriptBox["aMax", "2"], " ", + RowBox[{"p0", "-", "pf", "+", + RowBox[{"tf", " ", "v0"}]}], ")"}], "2"]}], "-", + SuperscriptBox[ RowBox[{"(", - RowBox[{ - RowBox[{"-", "v0"}], "+", "vf"}], ")"}]}]}], ")"}]}], "+", + RowBox[{"v0", "-", "vf"}], ")"}], "3"]}], ")"}]}], "+", RowBox[{"6", " ", + SuperscriptBox["af", "4"], " ", "jMax", " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}]}], "+", + RowBox[{"144", " ", "af", " ", + SuperscriptBox["jMax", "3"], " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf", "+", + RowBox[{"tf", " ", "v0"}]}], ")"}], " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}]}], "+", + RowBox[{"36", " ", SuperscriptBox["af", "2"], " ", + SuperscriptBox["jMax", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"]}], "+", + RowBox[{"9", " ", + SuperscriptBox["a0", "4"], " ", RowBox[{"(", RowBox[{ - SuperscriptBox["aMax", "2"], "+", + SuperscriptBox["af", "2"], "+", + RowBox[{"4", " ", "af", " ", "jMax", " ", "tf"}], "-", RowBox[{"2", " ", "jMax", " ", RowBox[{"(", RowBox[{ - RowBox[{"-", "v0"}], "+", "vf"}], ")"}]}]}], ")"}]}]}], + RowBox[{"jMax", " ", + SuperscriptBox["tf", "2"]}], "-", + RowBox[{"3", " ", "v0"}], "+", + RowBox[{"3", " ", "vf"}]}], ")"}]}]}], ")"}]}], "+", + RowBox[{"3", " ", + SuperscriptBox["a0", "2"], " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["af", "4"], "+", + RowBox[{"8", " ", + SuperscriptBox["af", "3"], " ", "jMax", " ", "tf"}], "-", + RowBox[{"24", " ", + SuperscriptBox["jMax", "3"], " ", "tf", " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf", "+", + RowBox[{"tf", " ", "v0"}]}], ")"}]}], "+", + RowBox[{"12", " ", + SuperscriptBox["af", "2"], " ", "jMax", " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}]}], "+", + RowBox[{"36", " ", + SuperscriptBox["jMax", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"]}], "+", + RowBox[{"24", " ", "af", " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf", "+", + RowBox[{"2", " ", "tf", " ", "v0"}], "-", + RowBox[{"tf", " ", "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.8175561893975782`*^9, 3.817556237683996*^9}, {3.8175565534030046`*^9, - 3.81755660742447*^9}}, + 3.81755660742447*^9}, {3.8176105219956827`*^9, 3.817610527078855*^9}, { + 3.817610627339719*^9, 3.817610664272493*^9}, {3.817610855954926*^9, + 3.817610865795534*^9}, {3.8176109215709476`*^9, 3.8176109399589663`*^9}}, CellLabel-> - "Out[148]=",ExpressionUUID->"2e4b2150-6f82-4c7d-8607-3649724667c6"] + "Out[879]=",ExpressionUUID->"73320609-b23d-4237-841c-1809a13b55cc"] }, Open ]], Cell[CellGroupData[{ @@ -940,10 +1001,10 @@ Cell[BoxData[{ RowBox[{"rootFunction", "\[Rule]", "t"}], "}"}]}]}], ";"}], "\[IndentingNewLine]", RowBox[{"tmp", "=", - RowBox[{"FullSimplify", "[", + RowBox[{"Simplify", "[", RowBox[{"tmpReplaced", "[", RowBox[{"[", - RowBox[{"7", ",", "2"}], "]"}], "]"}], "]"}]}], "\[IndentingNewLine]", + RowBox[{"4", ",", "2"}], "]"}], "]"}], "]"}]}], "\[IndentingNewLine]", RowBox[{ RowBox[{"ToString", "[", RowBox[{"tmp", ",", "CForm"}], "]"}], ";"}], "\[IndentingNewLine]", @@ -953,311 +1014,1346 @@ Cell[BoxData[{ 3.8175307010854073`*^9}, {3.817530733671631*^9, 3.817530733709938*^9}, { 3.8175349306462812`*^9, 3.8175349353248568`*^9}, {3.817534969175642*^9, 3.817534981662369*^9}, {3.8175562930510178`*^9, 3.817556322147118*^9}, { - 3.817556643892717*^9, 3.817556671396356*^9}}, - CellLabel-> - "In[175]:=",ExpressionUUID->"58c30e25-83c4-4d05-ad21-e88fcb7f4181"], - -Cell[BoxData[ - 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.817556293641871*^9, 3.817556322632617*^9}, {3.817556644430108*^9, - 3.8175566719195757`*^9}}, + 3.817556643892717*^9, 3.817556671396356*^9}, {3.8176107129056*^9, + 3.817610756006242*^9}, {3.817610989934662*^9, 3.817610996721653*^9}}, CellLabel-> - "Out[176]=",ExpressionUUID->"333c90a7-66b2-4ebd-90da-2deda377e542"] -}, Open ]], - -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"], + "In[894]:=",ExpressionUUID->"58c30e25-83c4-4d05-ad21-e88fcb7f4181"], Cell[BoxData[ RowBox[{ - RowBox[{"resultTa", "=", - RowBox[{"{", "\[IndentingNewLine]", + RowBox[{"(", + RowBox[{ RowBox[{ - RowBox[{"Solve", "[", + RowBox[{"-", "23"}], " ", + SuperscriptBox["a0", "7"]}], "+", + RowBox[{"3", " ", + SuperscriptBox["af", "7"]}], "+", + RowBox[{ + SuperscriptBox["af", "6"], " ", "jMax", " ", + RowBox[{"(", RowBox[{ - RowBox[{"{", - RowBox[{ - RowBox[{"a7", "\[Equal]", "0"}], ",", - RowBox[{"v7", "\[Equal]", "vf"}], ",", - RowBox[{"p7", "\[Equal]", "pf"}], ",", - RowBox[{"a3", "\[Equal]", "aPlat"}], ",", - RowBox[{"t2", "\[Equal]", "0"}], ",", - RowBox[{"t6", "\[Equal]", "0"}], ",", - RowBox[{"t7", "\[Equal]", "0"}], ",", - RowBox[{"tAll", "\[Equal]", "tf"}]}], "}"}], ",", - RowBox[{"Join", "[", - RowBox[{"tVars", ",", - RowBox[{"{", "aPlat", "}"}]}], "]"}], ",", - RowBox[{"Cubics", "\[Rule]", "False"}], ",", - RowBox[{"Quartics", "\[Rule]", "False"}]}], "]"}], ",", - "\[IndentingNewLine]", - RowBox[{"Solve", "[", + RowBox[{ + RowBox[{"-", "20"}], " ", "t"}], "+", + RowBox[{"9", " ", "tf"}]}], ")"}]}], "+", + RowBox[{ + SuperscriptBox["a0", "6"], " ", + RowBox[{"(", RowBox[{ - RowBox[{"{", - RowBox[{ - RowBox[{"a7", "\[Equal]", "0"}], ",", - RowBox[{"v7", "\[Equal]", "vf"}], ",", - RowBox[{"p7", "\[Equal]", "pf"}], ",", - RowBox[{"a3", "\[Equal]", - RowBox[{"-", "aPlat"}]}], ",", - RowBox[{"t1", "\[Equal]", "0"}], ",", - RowBox[{"t2", "\[Equal]", "0"}], ",", - RowBox[{"t6", "\[Equal]", "0"}], ",", - RowBox[{"tAll", "\[Equal]", "tf"}]}], "}"}], ",", - RowBox[{"Join", "[", - RowBox[{"tVars", ",", - RowBox[{"{", "aPlat", "}"}]}], "]"}], ",", - RowBox[{"Cubics", "\[Rule]", "False"}], ",", - RowBox[{"Quartics", "\[Rule]", "False"}]}], "]"}], ",", - "\[IndentingNewLine]", - RowBox[{"Solve", "[", + RowBox[{"39", " ", "af"}], "-", + RowBox[{"200", " ", "jMax", " ", "t"}], "-", + RowBox[{"39", " ", "jMax", " ", "tf"}]}], ")"}]}], "+", + RowBox[{"3", " ", + SuperscriptBox["a0", "5"], " ", + RowBox[{"(", RowBox[{ - RowBox[{"{", - RowBox[{ - RowBox[{"a7", "\[Equal]", "0"}], ",", - RowBox[{"v7", "\[Equal]", "vf"}], ",", - RowBox[{"p7", "\[Equal]", "pf"}], ",", - RowBox[{"a3", "\[Equal]", "aPlat"}], ",", - RowBox[{"a1", "\[Equal]", "aMax"}], ",", - RowBox[{"t6", "\[Equal]", "0"}], ",", - RowBox[{"t7", "\[Equal]", "0"}], ",", - RowBox[{"tAll", "\[Equal]", "tf"}]}], "}"}], ",", - RowBox[{"Join", "[", - RowBox[{"tVars", ",", - RowBox[{"{", "aPlat", "}"}]}], "]"}], ",", - RowBox[{"Cubics", "\[Rule]", "False"}], ",", - RowBox[{"Quartics", "\[Rule]", "False"}]}], "]"}], ",", - "\[IndentingNewLine]", - RowBox[{"Solve", "[", + RowBox[{"7", " ", + SuperscriptBox["af", "2"]}], "+", + RowBox[{"8", " ", "af", " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"11", " ", "t"}], "-", + RowBox[{"5", " ", "tf"}]}], ")"}]}], "-", + RowBox[{"2", " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"45", " ", + SuperscriptBox["t", "2"]}], "+", + RowBox[{"22", " ", "t", " ", "tf"}], "-", + RowBox[{"5", " ", + SuperscriptBox["tf", "2"]}]}], ")"}]}], "+", + RowBox[{"13", " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}]}]}], ")"}]}]}], ")"}]}], "+", + RowBox[{"6", " ", + SuperscriptBox["af", "5"], " ", "jMax", " ", + RowBox[{"(", RowBox[{ - RowBox[{"{", - RowBox[{ - RowBox[{"a7", "\[Equal]", "0"}], ",", - RowBox[{"v7", "\[Equal]", "vf"}], ",", - RowBox[{"p7", "\[Equal]", "pf"}], ",", - RowBox[{"a3", "\[Equal]", "aPlat"}], ",", - RowBox[{"t1", "\[Equal]", "0"}], ",", - RowBox[{"t2", "\[Equal]", "0"}], ",", - RowBox[{"a5", "\[Equal]", - RowBox[{"-", "aMax"}]}], ",", - RowBox[{"tAll", "\[Equal]", "tf"}]}], "}"}], ",", - RowBox[{"Join", "[", - RowBox[{"tVars", ",", - RowBox[{"{", "aPlat", "}"}]}], "]"}], ",", - RowBox[{"Cubics", "\[Rule]", "False"}], ",", - RowBox[{"Quartics", "\[Rule]", "False"}]}], "]"}], ",", - "\[IndentingNewLine]", - RowBox[{"Solve", "[", + RowBox[{"2", " ", "jMax", " ", + SuperscriptBox["t", "2"]}], "+", + RowBox[{"4", " ", "jMax", " ", "t", " ", "tf"}], "-", + RowBox[{"jMax", " ", + SuperscriptBox["tf", "2"]}], "+", + RowBox[{"5", " ", "v0"}], "-", + RowBox[{"5", " ", "vf"}]}], ")"}]}], "+", + RowBox[{"6", " ", + SuperscriptBox["af", "4"], " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", RowBox[{ - RowBox[{"{", - RowBox[{ - RowBox[{"a7", "\[Equal]", "0"}], ",", - RowBox[{"v7", "\[Equal]", "vf"}], ",", - RowBox[{"p7", "\[Equal]", "pf"}], ",", - RowBox[{"a3", "\[Equal]", "aPlat"}], ",", - RowBox[{"a1", "\[Equal]", - RowBox[{"-", "aMax"}]}], ",", - RowBox[{"t6", "\[Equal]", "0"}], ",", - RowBox[{"t7", "\[Equal]", "0"}], ",", - RowBox[{"tAll", "\[Equal]", "tf"}]}], "}"}], ",", - RowBox[{"Join", "[", - RowBox[{"tVars", ",", - RowBox[{"{", "aPlat", "}"}]}], "]"}], ",", - RowBox[{"Cubics", "\[Rule]", "False"}], ",", - RowBox[{"Quartics", "\[Rule]", "False"}]}], "]"}], ",", - "\[IndentingNewLine]", - RowBox[{"Solve", "[", + RowBox[{ + RowBox[{"-", "12"}], " ", "p0"}], "+", + RowBox[{"12", " ", "pf"}], "+", + RowBox[{"14", " ", "jMax", " ", + SuperscriptBox["t", "3"]}], "-", + RowBox[{"14", " ", "jMax", " ", + SuperscriptBox["t", "2"], " ", "tf"}], "-", + RowBox[{"6", " ", "jMax", " ", "t", " ", + SuperscriptBox["tf", "2"]}], "+", + RowBox[{"3", " ", "jMax", " ", + SuperscriptBox["tf", "3"]}], "-", + RowBox[{"16", " ", "t", " ", "v0"}], "-", + RowBox[{"5", " ", "tf", " ", "v0"}], "+", + RowBox[{"16", " ", "t", " ", "vf"}], "-", + RowBox[{"7", " ", "tf", " ", "vf"}]}], ")"}]}], "-", + RowBox[{"12", " ", + SuperscriptBox["af", "3"], " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", RowBox[{ - RowBox[{"{", - RowBox[{ - RowBox[{"a7", "\[Equal]", "0"}], ",", - RowBox[{"v7", "\[Equal]", "vf"}], ",", - RowBox[{"p7", "\[Equal]", "pf"}], ",", - RowBox[{"a3", "\[Equal]", "aPlat"}], ",", - RowBox[{"t1", "\[Equal]", "0"}], ",", - RowBox[{"t2", "\[Equal]", "0"}], ",", - RowBox[{"a5", "\[Equal]", "aMax"}], ",", - RowBox[{"tAll", "\[Equal]", "tf"}]}], "}"}], ",", - RowBox[{"Join", "[", - RowBox[{"tVars", ",", - RowBox[{"{", "aPlat", "}"}]}], "]"}], ",", - RowBox[{"Cubics", "\[Rule]", "False"}], ",", - RowBox[{"Quartics", "\[Rule]", "False"}]}], "]"}]}], - "\[IndentingNewLine]", "}"}]}], ";"}]], "Input", - CellChangeTimes->{{3.817199307345278*^9, 3.817199324696947*^9}, - 3.817200448131874*^9, {3.817202061689239*^9, 3.817202085862195*^9}, { - 3.817265456500353*^9, 3.817265494996078*^9}, {3.817265620283185*^9, - 3.817265622078382*^9}, {3.817266190182694*^9, 3.81726622245671*^9}, { - 3.8172663642517967`*^9, 3.817266364294613*^9}, {3.817266770950122*^9, - 3.817266786651701*^9}, 3.817266823427422*^9, {3.817267394227706*^9, - 3.817267397242619*^9}, {3.8172674685429163`*^9, 3.817267490207313*^9}, - 3.8172675237936296`*^9, 3.817269006850615*^9, 3.817269180494939*^9, { - 3.8172693796870193`*^9, 3.81726939540921*^9}, 3.817269489019843*^9, { - 3.817269550913919*^9, 3.817269582495365*^9}, {3.8172696597792683`*^9, - 3.817269673093995*^9}}, - CellLabel-> - "In[271]:=",ExpressionUUID->"013ba19d-162c-4e14-9e6b-43bcf125ceef"], - -Cell["\<\ -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}, {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[{"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, - 3.817265763955217*^9}, {3.8172658406982603`*^9, 3.8172658423822193`*^9}, { - 3.8172662344229*^9, 3.817266284843684*^9}, {3.817266331152025*^9, - 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}, {3.817558355798354*^9, - 3.817558365794586*^9}},ExpressionUUID->"b4fd3303-7c30-4af8-94b1-\ -4505577eaf40"], - -Cell[BoxData["\<\"aMax/jMax\"\>"], "Output", - CellChangeTimes->{ - 3.817265503359898*^9, {3.817265635594121*^9, 3.817265642807448*^9}, { - 3.817265717144732*^9, 3.817265764334104*^9}, {3.8172658410199757`*^9, - 3.817265843063768*^9}, {3.817266228363278*^9, 3.817266285241193*^9}, - 3.817266336850995*^9, {3.817266389245998*^9, 3.817266447095387*^9}, { - 3.817266847014901*^9, 3.817266896239656*^9}, {3.817268768162199*^9, - 3.817268772746282*^9}, {3.817268819523574*^9, 3.8172688811055727`*^9}, { - 3.817269691950478*^9, 3.817269741332467*^9}}, - CellLabel-> - "Out[689]=",ExpressionUUID->"3c247a78-5f1f-4bfe-aeb8-c0def9d99fe4"] -}, Open ]], - -Cell["Case 1: Root is t1, Case 2: Root is t3", "Text", - CellChangeTimes->{{3.8172021457068987`*^9, - 3.817202155527573*^9}},ExpressionUUID->"6e1bb727-a1dc-4c24-af7f-\ -abfa0f3b9409"], + RowBox[{"2", " ", + SuperscriptBox["jMax", "2"], " ", + SuperscriptBox["t", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"6", " ", + SuperscriptBox["t", "2"]}], "+", + RowBox[{"2", " ", "t", " ", "tf"}], "-", + RowBox[{"7", " ", + SuperscriptBox["tf", "2"]}]}], ")"}]}], "-", + RowBox[{"5", " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"]}], "+", + RowBox[{"2", " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "pf", " ", "t"}], "+", + RowBox[{"6", " ", "pf", " ", "tf"}], "-", + RowBox[{"2", " ", "p0", " ", + RowBox[{"(", + RowBox[{"t", "+", + RowBox[{"3", " ", "tf"}]}], ")"}]}], "+", + RowBox[{ + SuperscriptBox["t", "2"], " ", "v0"}], "-", + RowBox[{"4", " ", "t", " ", "tf", " ", "v0"}], "-", + RowBox[{"7", " ", + SuperscriptBox["tf", "2"], " ", "v0"}], "-", + RowBox[{ + SuperscriptBox["t", "2"], " ", "vf"}], "+", + RowBox[{"2", " ", "t", " ", "tf", " ", "vf"}], "+", + RowBox[{ + SuperscriptBox["tf", "2"], " ", "vf"}]}], ")"}]}]}], ")"}]}], "+", + + RowBox[{"36", " ", + SuperscriptBox["af", "2"], " ", + SuperscriptBox["jMax", "3"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", + SuperscriptBox["jMax", "2"], " ", + SuperscriptBox["t", "2"], " ", "tf", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", + SuperscriptBox["t", "2"]}], "+", + RowBox[{"t", " ", "tf"}], "-", + SuperscriptBox["tf", "2"]}], ")"}]}], "-", + RowBox[{ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"4", " ", "p0"}], "-", + RowBox[{"4", " ", "pf"}], "+", + RowBox[{"4", " ", "t", " ", "v0"}], "-", + RowBox[{"tf", " ", "v0"}], "-", + RowBox[{"4", " ", "t", " ", "vf"}], "+", + RowBox[{"5", " ", "tf", " ", "vf"}]}], ")"}]}], "+", + RowBox[{"2", " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"p0", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["t", "2"], "+", + RowBox[{"2", " ", "t", " ", "tf"}], "-", + RowBox[{"2", " ", + SuperscriptBox["tf", "2"]}]}], ")"}]}], "-", + RowBox[{"pf", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["t", "2"], "+", + RowBox[{"2", " ", "t", " ", "tf"}], "-", + RowBox[{"2", " ", + SuperscriptBox["tf", "2"]}]}], ")"}]}], "+", + RowBox[{"4", " ", + SuperscriptBox["t", "3"], " ", "v0"}], "+", + RowBox[{ + SuperscriptBox["t", "2"], " ", "tf", " ", "v0"}], "-", + RowBox[{ + SuperscriptBox["tf", "3"], " ", "v0"}], "-", + RowBox[{"4", " ", + SuperscriptBox["t", "3"], " ", "vf"}], "+", + RowBox[{"2", " ", "t", " ", + SuperscriptBox["tf", "2"], " ", "vf"}], "-", + RowBox[{ + SuperscriptBox["tf", "3"], " ", "vf"}]}], ")"}]}]}], ")"}]}], "+", + + RowBox[{ + SuperscriptBox["a0", "4"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "65"}], " ", + SuperscriptBox["af", "3"]}], "+", + RowBox[{"3", " ", + SuperscriptBox["af", "2"], " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"68", " ", "t"}], "+", + RowBox[{"35", " ", "tf"}]}], ")"}]}], "-", + RowBox[{"6", " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"20", " ", "p0"}], "-", + RowBox[{"20", " ", "pf"}], "+", + RowBox[{"82", " ", "jMax", " ", + SuperscriptBox["t", "3"]}], "+", + RowBox[{"104", " ", "jMax", " ", + SuperscriptBox["t", "2"], " ", "tf"}], "-", + RowBox[{"46", " ", "jMax", " ", "t", " ", + SuperscriptBox["tf", "2"]}], "+", + RowBox[{"24", " ", "t", " ", "v0"}], "-", + RowBox[{"15", " ", "tf", " ", "v0"}], "-", + RowBox[{"24", " ", "t", " ", "vf"}], "+", + RowBox[{"35", " ", "tf", " ", "vf"}]}], ")"}]}], "+", + RowBox[{"6", " ", "af", " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"4", " ", "jMax", " ", "t", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"26", " ", "t"}], "-", + RowBox[{"23", " ", "tf"}]}], ")"}]}], "+", + RowBox[{"15", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "v0"}], "+", "vf"}], ")"}]}]}], ")"}]}]}], ")"}]}], + "+", + RowBox[{ + SuperscriptBox["a0", "3"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"21", " ", + SuperscriptBox["af", "4"]}], "-", + RowBox[{"8", " ", + SuperscriptBox["af", "3"], " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"49", " ", "t"}], "-", + RowBox[{"15", " ", "tf"}]}], ")"}]}], "+", + RowBox[{"12", " ", + SuperscriptBox["af", "2"], " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"47", " ", + SuperscriptBox["t", "2"]}], "+", + RowBox[{"46", " ", "t", " ", "tf"}], "-", + RowBox[{"7", " ", + SuperscriptBox["tf", "2"]}]}], ")"}]}], "+", + RowBox[{"17", " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}]}]}], ")"}]}], "+", + RowBox[{"48", " ", "af", " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "3"}], " ", "p0"}], "+", + RowBox[{"3", " ", "pf"}], "+", + RowBox[{"11", " ", "jMax", " ", + SuperscriptBox["t", "3"]}], "-", + RowBox[{"11", " ", "jMax", " ", + SuperscriptBox["t", "2"], " ", "tf"}], "-", + RowBox[{"13", " ", "t", " ", "v0"}], "+", + RowBox[{"tf", " ", "v0"}], "+", + RowBox[{"13", " ", "t", " ", "vf"}], "-", + RowBox[{"4", " ", "tf", " ", "vf"}]}], ")"}]}], "-", + RowBox[{"12", " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", + SuperscriptBox["jMax", "2"], " ", + SuperscriptBox["t", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"6", " ", + SuperscriptBox["t", "2"]}], "+", + RowBox[{"22", " ", "t", " ", "tf"}], "-", + RowBox[{"11", " ", + SuperscriptBox["tf", "2"]}]}], ")"}]}], "-", + RowBox[{"19", " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"]}], "+", + RowBox[{"2", " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"20", " ", "p0", " ", "t"}], "-", + RowBox[{"20", " ", "pf", " ", "t"}], "-", + RowBox[{"6", " ", "p0", " ", "tf"}], "+", + RowBox[{"6", " ", "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[{"46", " ", "t", " ", "tf", " ", "vf"}], "-", + RowBox[{"7", " ", + SuperscriptBox["tf", "2"], " ", "vf"}]}], ")"}]}]}], ")"}]}]}], + ")"}]}], "+", + RowBox[{"3", " ", + SuperscriptBox["a0", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"5", " ", + SuperscriptBox["af", "5"]}], "+", + RowBox[{ + SuperscriptBox["af", "4"], " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"32", " ", "t"}], "-", + RowBox[{"29", " ", "tf"}]}], ")"}]}], "-", + RowBox[{"4", " ", + SuperscriptBox["af", "3"], " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"69", " ", + SuperscriptBox["t", "2"]}], "-", + RowBox[{"40", " ", "t", " ", "tf"}], "-", + RowBox[{"7", " ", + SuperscriptBox["tf", "2"]}]}], ")"}]}], "+", "v0", "-", "vf"}], + ")"}]}], "+", + RowBox[{"12", " ", + SuperscriptBox["af", "2"], " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"4", " ", "p0"}], "-", + RowBox[{"4", " ", "pf"}], "+", + RowBox[{"14", " ", "jMax", " ", + SuperscriptBox["t", "3"]}], "+", + RowBox[{"29", " ", "jMax", " ", + SuperscriptBox["t", "2"], " ", "tf"}], "-", + RowBox[{"6", " ", "jMax", " ", "t", " ", + SuperscriptBox["tf", "2"]}], "-", + RowBox[{"jMax", " ", + SuperscriptBox["tf", "3"]}], "+", + RowBox[{"24", " ", "t", " ", "v0"}], "-", + RowBox[{"tf", " ", "v0"}], "-", + RowBox[{"24", " ", "t", " ", "vf"}], "+", + RowBox[{"5", " ", "tf", " ", "vf"}]}], ")"}]}], "+", + RowBox[{"12", " ", "af", " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"4", " ", + SuperscriptBox["jMax", "2"], " ", + SuperscriptBox["t", "3"], " ", + RowBox[{"(", + RowBox[{"t", "-", "tf"}], ")"}]}], "-", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"], "-", + RowBox[{"4", " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"pf", " ", "t"}], "-", + RowBox[{"pf", " ", "tf"}], "+", + RowBox[{"p0", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "t"}], "+", "tf"}], ")"}]}], "+", + RowBox[{"10", " ", + SuperscriptBox["t", "2"], " ", "v0"}], "-", + RowBox[{"8", " ", "t", " ", "tf", " ", "v0"}], "-", + RowBox[{"10", " ", + SuperscriptBox["t", "2"], " ", "vf"}], "+", + RowBox[{"7", " ", "t", " ", "tf", " ", "vf"}], "+", + RowBox[{ + SuperscriptBox["tf", "2"], " ", "vf"}]}], ")"}]}]}], ")"}]}], + "-", + RowBox[{"12", " ", + SuperscriptBox["jMax", "3"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", + SuperscriptBox["jMax", "2"], " ", + SuperscriptBox["t", "3"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "t"}], "-", "tf"}], ")"}], " ", "tf"}], "-", + RowBox[{ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"4", " ", "p0"}], "-", + RowBox[{"4", " ", "pf"}], "+", + RowBox[{"40", " ", "t", " ", "v0"}], "+", + RowBox[{"tf", " ", "v0"}], "-", + RowBox[{"40", " ", "t", " ", "vf"}], "+", + RowBox[{"3", " ", "tf", " ", "vf"}]}], ")"}]}], "-", + RowBox[{"2", " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"9", " ", "pf", " ", + SuperscriptBox["t", "2"]}], "+", + RowBox[{"2", " ", "pf", " ", "t", " ", "tf"}], "-", + RowBox[{"pf", " ", + SuperscriptBox["tf", "2"]}], "+", + RowBox[{"p0", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "9"}], " ", + SuperscriptBox["t", "2"]}], "-", + RowBox[{"2", " ", "t", " ", "tf"}], "+", + SuperscriptBox["tf", "2"]}], ")"}]}], "+", + RowBox[{"12", " ", + SuperscriptBox["t", "3"], " ", "v0"}], "+", + RowBox[{"20", " ", + SuperscriptBox["t", "2"], " ", "tf", " ", "v0"}], "-", + RowBox[{"8", " ", "t", " ", + SuperscriptBox["tf", "2"], " ", "v0"}], "-", + RowBox[{"12", " ", + SuperscriptBox["t", "3"], " ", "vf"}], "-", + RowBox[{"29", " ", + SuperscriptBox["t", "2"], " ", "tf", " ", "vf"}], "+", + RowBox[{"6", " ", "t", " ", + SuperscriptBox["tf", "2"], " ", "vf"}], "+", + RowBox[{ + SuperscriptBox["tf", "3"], " ", "vf"}]}], ")"}]}]}], ")"}]}]}], + ")"}]}], "+", + RowBox[{"72", " ", "af", " ", + SuperscriptBox["jMax", "3"], " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "3"], "+", + RowBox[{"4", " ", + SuperscriptBox["jMax", "2"], " ", + SuperscriptBox["t", "2"], " ", + RowBox[{"(", + RowBox[{"t", "-", "tf"}], ")"}], " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf", "-", + RowBox[{"t", " ", "v0"}], "+", + RowBox[{"t", " ", "vf"}], "+", + RowBox[{"tf", " ", "vf"}]}], ")"}]}], "+", + RowBox[{"jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "3"}], " ", + SuperscriptBox["p0", "2"]}], "-", + RowBox[{"3", " ", + SuperscriptBox["pf", "2"]}], "+", + RowBox[{"pf", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "4"}], " ", "t", " ", "v0"}], "+", + RowBox[{"4", " ", "tf", " ", "v0"}], "+", + RowBox[{"4", " ", "t", " ", "vf"}], "+", + RowBox[{"2", " ", "tf", " ", "vf"}]}], ")"}]}], "+", + RowBox[{"tf", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"4", " ", "t", " ", "v0", " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}]}], "+", + RowBox[{"tf", " ", "vf", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "4"}], " ", "v0"}], "+", "vf"}], ")"}]}]}], + ")"}]}], "+", + RowBox[{"p0", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"6", " ", "pf"}], "+", + RowBox[{"4", " ", "t", " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}]}], "-", + RowBox[{"2", " ", "tf", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "v0"}], "+", "vf"}], ")"}]}]}], ")"}]}]}], + ")"}]}]}], ")"}]}], "-", + RowBox[{"72", " ", + SuperscriptBox["jMax", "4"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", + SuperscriptBox["jMax", "2"], " ", + SuperscriptBox["t", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "t"}], "-", "tf"}], ")"}], " ", "tf", " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf", "-", + RowBox[{"t", " ", "v0"}], "+", + RowBox[{"t", " ", "vf"}], "+", + RowBox[{"tf", " ", "vf"}]}], ")"}]}], "+", + RowBox[{ + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"4", " ", "p0"}], "-", + RowBox[{"4", " ", "pf"}], "+", + RowBox[{"tf", " ", + RowBox[{"(", + RowBox[{"v0", "+", + RowBox[{"3", " ", "vf"}]}], ")"}]}]}], ")"}]}], "+", + RowBox[{"jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + SuperscriptBox["p0", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"4", " ", "t"}], "-", + RowBox[{"3", " ", "tf"}]}], ")"}]}], "+", + RowBox[{ + SuperscriptBox["pf", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"4", " ", "t"}], "-", + RowBox[{"3", " ", "tf"}]}], ")"}]}], "-", + RowBox[{"6", " ", + SuperscriptBox["t", "3"], " ", + SuperscriptBox["v0", "2"]}], "+", + RowBox[{"2", " ", "t", " ", + SuperscriptBox["tf", "2"], " ", + SuperscriptBox["v0", "2"]}], "+", + RowBox[{"12", " ", + SuperscriptBox["t", "3"], " ", "v0", " ", "vf"}], "+", + RowBox[{"2", " ", + SuperscriptBox["t", "2"], " ", "tf", " ", "v0", " ", "vf"}], "-", + RowBox[{"2", " ", + SuperscriptBox["tf", "3"], " ", "v0", " ", "vf"}], "-", + RowBox[{"6", " ", + SuperscriptBox["t", "3"], " ", + SuperscriptBox["vf", "2"]}], "-", + RowBox[{"2", " ", + SuperscriptBox["t", "2"], " ", "tf", " ", + SuperscriptBox["vf", "2"]}], "+", + RowBox[{"2", " ", "t", " ", + SuperscriptBox["tf", "2"], " ", + SuperscriptBox["vf", "2"]}], "-", + RowBox[{ + SuperscriptBox["tf", "3"], " ", + SuperscriptBox["vf", "2"]}], "-", + RowBox[{"2", " ", "p0", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"pf", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"4", " ", "t"}], "-", + RowBox[{"3", " ", "tf"}]}], ")"}]}], "+", + RowBox[{ + SuperscriptBox["t", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "v0"}], "+", "vf"}], ")"}]}], "-", + RowBox[{"2", " ", "t", " ", "tf", " ", + RowBox[{"(", + RowBox[{"v0", "+", "vf"}], ")"}]}], "+", + RowBox[{ + SuperscriptBox["tf", "2"], " ", + RowBox[{"(", + RowBox[{"v0", "+", + RowBox[{"2", " ", "vf"}]}], ")"}]}]}], ")"}]}], "+", + RowBox[{"pf", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "2"}], " ", + SuperscriptBox["t", "2"], " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}]}], "-", + RowBox[{"4", " ", "t", " ", "tf", " ", + RowBox[{"(", + RowBox[{"v0", "+", "vf"}], ")"}]}], "+", + RowBox[{"2", " ", + SuperscriptBox["tf", "2"], " ", + RowBox[{"(", + RowBox[{"v0", "+", + RowBox[{"2", " ", "vf"}]}], ")"}]}]}], ")"}]}]}], ")"}]}]}], + ")"}]}], "+", + RowBox[{"a0", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "11"}], " ", + SuperscriptBox["af", "6"]}], "+", + RowBox[{"12", " ", + SuperscriptBox["af", "5"], " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"4", " ", "t"}], "+", "tf"}], ")"}]}], "+", + RowBox[{"6", " ", + SuperscriptBox["af", "4"], " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"28", " ", + SuperscriptBox["t", "2"]}], "-", + RowBox[{"40", " ", "t", " ", "tf"}], "+", + RowBox[{"3", " ", + SuperscriptBox["tf", "2"]}]}], ")"}]}], "-", + RowBox[{"9", " ", "v0"}], "+", + RowBox[{"9", " ", "vf"}]}], ")"}]}], "-", + RowBox[{"48", " ", + SuperscriptBox["af", "3"], " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "4"}], " ", "p0"}], "+", + RowBox[{"4", " ", "pf"}], "+", + RowBox[{"13", " ", "jMax", " ", + SuperscriptBox["t", "3"]}], "-", + RowBox[{"4", " ", "jMax", " ", + SuperscriptBox["t", "2"], " ", "tf"}], "-", + RowBox[{"7", " ", "jMax", " ", "t", " ", + SuperscriptBox["tf", "2"]}], "-", + RowBox[{"6", " ", "tf", " ", "v0"}], "+", + RowBox[{"2", " ", "tf", " ", "vf"}]}], ")"}]}], "+", + RowBox[{"144", " ", "af", " ", + SuperscriptBox["jMax", "3"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"3", " ", "p0"}], "-", + RowBox[{"3", " ", "pf"}], "+", + RowBox[{"4", " ", "tf", " ", "v0"}], "-", + RowBox[{"tf", " ", "vf"}]}], ")"}]}], "-", + RowBox[{"4", " ", "jMax", " ", "t", " ", + RowBox[{"(", + RowBox[{"t", "-", "tf"}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "p0"}], "+", "pf", "+", + RowBox[{"2", " ", "t", " ", "v0"}], "-", + RowBox[{"2", " ", "t", " ", "vf"}], "-", + RowBox[{"tf", " ", "vf"}]}], ")"}]}]}], ")"}]}], "+", + RowBox[{"36", " ", + SuperscriptBox["af", "2"], " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"4", " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["t", "4"], "+", + RowBox[{"5", " ", + SuperscriptBox["t", "3"], " ", "tf"}], "-", + RowBox[{"t", " ", + SuperscriptBox["tf", "3"]}]}], ")"}]}], "+", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"], "+", + RowBox[{"jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"8", " ", "p0", " ", "t"}], "-", + RowBox[{"8", " ", "pf", " ", "t"}], "-", + RowBox[{"4", " ", "p0", " ", "tf"}], "+", + RowBox[{"4", " ", "pf", " ", "tf"}], "+", + RowBox[{"24", " ", + SuperscriptBox["t", "2"], " ", "v0"}], "-", + RowBox[{"2", " ", + SuperscriptBox["tf", "2"], " ", "v0"}], "-", + RowBox[{"24", " ", + SuperscriptBox["t", "2"], " ", "vf"}], "+", + RowBox[{"8", " ", "t", " ", "tf", " ", "vf"}], "-", + RowBox[{"2", " ", + SuperscriptBox["tf", "2"], " ", "vf"}]}], ")"}]}]}], ")"}]}], + "-", + RowBox[{"72", " ", + SuperscriptBox["jMax", "3"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "3"}], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "3"]}], "-", + RowBox[{"4", " ", + SuperscriptBox["jMax", "2"], " ", "t", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"pf", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["t", "2"], "+", + RowBox[{"2", " ", "t", " ", "tf"}], "-", + SuperscriptBox["tf", "2"]}], ")"}]}], "+", + RowBox[{"p0", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + SuperscriptBox["t", "2"]}], "-", + RowBox[{"2", " ", "t", " ", "tf"}], "+", + SuperscriptBox["tf", "2"]}], ")"}]}], "+", + RowBox[{ + SuperscriptBox["t", "3"], " ", "v0"}], "+", + RowBox[{"4", " ", + SuperscriptBox["t", "2"], " ", "tf", " ", "v0"}], "-", + RowBox[{"2", " ", "t", " ", + SuperscriptBox["tf", "2"], " ", "v0"}], "-", + RowBox[{ + SuperscriptBox["t", "3"], " ", "vf"}], "-", + RowBox[{"5", " ", + SuperscriptBox["t", "2"], " ", "tf", " ", "vf"}], "+", + RowBox[{ + SuperscriptBox["tf", "3"], " ", "vf"}]}], ")"}]}], "+", + RowBox[{"jMax", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["p0", "2"], "+", + SuperscriptBox["pf", "2"], "-", + RowBox[{"6", " ", "pf", " ", "tf", " ", "v0"}], "-", + RowBox[{"20", " ", + SuperscriptBox["t", "2"], " ", + SuperscriptBox["v0", "2"]}], "+", + RowBox[{"4", " ", + SuperscriptBox["tf", "2"], " ", + SuperscriptBox["v0", "2"]}], "+", + RowBox[{"4", " ", "pf", " ", "tf", " ", "vf"}], "+", + RowBox[{"40", " ", + SuperscriptBox["t", "2"], " ", "v0", " ", "vf"}], "-", + RowBox[{"2", " ", + SuperscriptBox["tf", "2"], " ", "v0", " ", "vf"}], "-", + RowBox[{"20", " ", + SuperscriptBox["t", "2"], " ", + SuperscriptBox["vf", "2"]}], "-", + RowBox[{ + SuperscriptBox["tf", "2"], " ", + SuperscriptBox["vf", "2"]}], "-", + RowBox[{"2", " ", "p0", " ", + RowBox[{"(", + RowBox[{"pf", "-", + RowBox[{"3", " ", "tf", " ", "v0"}], "+", + RowBox[{"2", " ", "tf", " ", "vf"}]}], ")"}]}]}], ")"}]}]}], + ")"}]}]}], ")"}]}]}], ")"}], "/", + RowBox[{"(", + RowBox[{"jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + SuperscriptBox["a0", "6"]}], "+", + RowBox[{"11", " ", + SuperscriptBox["af", "6"]}], "-", + RowBox[{"12", " ", + SuperscriptBox["af", "5"], " ", "jMax", " ", "tf"}], "+", + RowBox[{"3", " ", + SuperscriptBox["a0", "4"], " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["af", "2"], "+", + RowBox[{"2", " ", "jMax", " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}]}]}], ")"}]}], "-", + RowBox[{"9", " ", + SuperscriptBox["a0", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["af", "2"], "+", + RowBox[{"2", " ", "jMax", " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}]}]}], ")"}], "2"]}], "+", + RowBox[{"18", " ", + SuperscriptBox["af", "4"], " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"jMax", " ", + SuperscriptBox["tf", "2"]}], "+", + RowBox[{"3", " ", "v0"}], "-", + RowBox[{"3", " ", "vf"}]}], ")"}]}], "+", + RowBox[{"24", " ", + SuperscriptBox["af", "3"], " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf", "+", + RowBox[{"tf", " ", "vf"}]}], ")"}]}], "+", + RowBox[{"8", " ", + SuperscriptBox["a0", "3"], " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["af", "3"], "-", + RowBox[{"3", " ", + SuperscriptBox["af", "2"], " ", "jMax", " ", "tf"}], "+", + RowBox[{"6", " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf", "+", + RowBox[{"tf", " ", "vf"}]}], ")"}]}]}], ")"}]}], "-", + RowBox[{"12", " ", "a0", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["af", "2"], "+", + RowBox[{"2", " ", "jMax", " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["af", "3"], "-", + RowBox[{"3", " ", + SuperscriptBox["af", "2"], " ", "jMax", " ", "tf"}], "+", + RowBox[{"6", " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf", "+", + RowBox[{"tf", " ", "vf"}]}], ")"}]}]}], ")"}]}], "-", + RowBox[{"36", " ", + SuperscriptBox["af", "2"], " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "3"}], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"]}], "+", + RowBox[{"2", " ", "jMax", " ", "tf", " ", + 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", + 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.817556293641871*^9, 3.817556322632617*^9}, {3.817556644430108*^9, + 3.8175566719195757`*^9}, {3.817610713426158*^9, 3.817610759093882*^9}, { + 3.81761099037247*^9, 3.817611001104216*^9}}, + CellLabel-> + "Out[895]=",ExpressionUUID->"db1e6e47-7503-4b17-aaaf-3bbf9d9ffecb"] +}, Open ]], + +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[ + RowBox[{ + RowBox[{"resultTa", "=", + RowBox[{"{", "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Solve", "[", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"a7", "\[Equal]", "af"}], ",", + RowBox[{"v7", "\[Equal]", "vf"}], ",", + RowBox[{"p7", "\[Equal]", "pf"}], ",", + RowBox[{"a3", "\[Equal]", "aPlat"}], ",", + RowBox[{"t2", "\[Equal]", "0"}], ",", + RowBox[{"t6", "\[Equal]", "0"}], ",", + RowBox[{"t7", "\[Equal]", "0"}], ",", + RowBox[{"tAll", "\[Equal]", "tf"}]}], "}"}], ",", + RowBox[{"Join", "[", + RowBox[{"tVars", ",", + RowBox[{"{", "aPlat", "}"}]}], "]"}], ",", + RowBox[{"Cubics", "\[Rule]", "False"}], ",", + RowBox[{"Quartics", "\[Rule]", "False"}]}], "]"}], ",", + "\[IndentingNewLine]", + RowBox[{"Solve", "[", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"a7", "\[Equal]", "af"}], ",", + RowBox[{"v7", "\[Equal]", "vf"}], ",", + RowBox[{"p7", "\[Equal]", "pf"}], ",", + RowBox[{"a3", "\[Equal]", + RowBox[{"-", "aPlat"}]}], ",", + RowBox[{"t4", "\[Equal]", "0"}], ",", + RowBox[{"t6", "\[Equal]", "0"}], ",", + RowBox[{"t7", "\[Equal]", "0"}], ",", + RowBox[{"tAll", "\[Equal]", "tf"}]}], "}"}], ",", + RowBox[{"Join", "[", + RowBox[{"tVars", ",", + RowBox[{"{", "aPlat", "}"}]}], "]"}], ",", + RowBox[{"Cubics", "\[Rule]", "False"}], ",", + RowBox[{"Quartics", "\[Rule]", "False"}]}], "]"}], ",", + "\[IndentingNewLine]", + RowBox[{"Solve", "[", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"a7", "\[Equal]", "af"}], ",", + RowBox[{"v7", "\[Equal]", "vf"}], ",", + RowBox[{"p7", "\[Equal]", "pf"}], ",", + RowBox[{"a3", "\[Equal]", "aPlat"}], ",", + RowBox[{"a1", "\[Equal]", "aMax"}], ",", + RowBox[{"t6", "\[Equal]", "0"}], ",", + RowBox[{"t7", "\[Equal]", "0"}], ",", + RowBox[{"tAll", "\[Equal]", "tf"}]}], "}"}], ",", + RowBox[{"Join", "[", + RowBox[{"tVars", ",", + RowBox[{"{", "aPlat", "}"}]}], "]"}], ",", + RowBox[{"Cubics", "\[Rule]", "False"}], ",", + RowBox[{"Quartics", "\[Rule]", "False"}]}], "]"}], ",", + "\[IndentingNewLine]", + RowBox[{"Solve", "[", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"a7", "\[Equal]", "af"}], ",", + RowBox[{"v7", "\[Equal]", "vf"}], ",", + RowBox[{"p7", "\[Equal]", "pf"}], ",", + RowBox[{"a3", "\[Equal]", "aPlat"}], ",", + RowBox[{"t1", "\[Equal]", "0"}], ",", + RowBox[{"t2", "\[Equal]", "0"}], ",", + RowBox[{"a5", "\[Equal]", + RowBox[{"-", "aMax"}]}], ",", + RowBox[{"tAll", "\[Equal]", "tf"}]}], "}"}], ",", + RowBox[{"Join", "[", + RowBox[{"tVars", ",", + RowBox[{"{", "aPlat", "}"}]}], "]"}], ",", + RowBox[{"Cubics", "\[Rule]", "False"}], ",", + RowBox[{"Quartics", "\[Rule]", "False"}]}], "]"}], ",", + "\[IndentingNewLine]", + RowBox[{"Solve", "[", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"a7", "\[Equal]", "af"}], ",", + RowBox[{"v7", "\[Equal]", "vf"}], ",", + RowBox[{"p7", "\[Equal]", "pf"}], ",", + RowBox[{"a3", "\[Equal]", "aPlat"}], ",", + RowBox[{"a1", "\[Equal]", + RowBox[{"-", "aMax"}]}], ",", + RowBox[{"t6", "\[Equal]", "0"}], ",", + RowBox[{"t7", "\[Equal]", "0"}], ",", + RowBox[{"tAll", "\[Equal]", "tf"}]}], "}"}], ",", + RowBox[{"Join", "[", + RowBox[{"tVars", ",", + RowBox[{"{", "aPlat", "}"}]}], "]"}], ",", + RowBox[{"Cubics", "\[Rule]", "False"}], ",", + RowBox[{"Quartics", "\[Rule]", "False"}]}], "]"}], ",", + "\[IndentingNewLine]", + RowBox[{"Solve", "[", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"a7", "\[Equal]", "af"}], ",", + RowBox[{"v7", "\[Equal]", "vf"}], ",", + RowBox[{"p7", "\[Equal]", "pf"}], ",", + RowBox[{"a3", "\[Equal]", "aPlat"}], ",", + RowBox[{"t1", "\[Equal]", "0"}], ",", + RowBox[{"t2", "\[Equal]", "0"}], ",", + RowBox[{"a5", "\[Equal]", "aMax"}], ",", + RowBox[{"tAll", "\[Equal]", "tf"}]}], "}"}], ",", + RowBox[{"Join", "[", + RowBox[{"tVars", ",", + RowBox[{"{", "aPlat", "}"}]}], "]"}], ",", + RowBox[{"Cubics", "\[Rule]", "False"}], ",", + RowBox[{"Quartics", "\[Rule]", "False"}]}], "]"}]}], + "\[IndentingNewLine]", "}"}]}], ";"}]], "Input", + CellChangeTimes->{{3.817199307345278*^9, 3.817199324696947*^9}, + 3.817200448131874*^9, {3.817202061689239*^9, 3.817202085862195*^9}, { + 3.817265456500353*^9, 3.817265494996078*^9}, {3.817265620283185*^9, + 3.817265622078382*^9}, {3.817266190182694*^9, 3.81726622245671*^9}, { + 3.8172663642517967`*^9, 3.817266364294613*^9}, {3.817266770950122*^9, + 3.817266786651701*^9}, 3.817266823427422*^9, {3.817267394227706*^9, + 3.817267397242619*^9}, {3.8172674685429163`*^9, 3.817267490207313*^9}, + 3.8172675237936296`*^9, 3.817269006850615*^9, 3.817269180494939*^9, { + 3.8172693796870193`*^9, 3.81726939540921*^9}, 3.817269489019843*^9, { + 3.817269550913919*^9, 3.817269582495365*^9}, {3.8172696597792683`*^9, + 3.817269673093995*^9}, {3.817558484371955*^9, 3.817558493709511*^9}, { + 3.8175614958931913`*^9, 3.81756151398792*^9}, {3.817561699269456*^9, + 3.8175617026592703`*^9}, {3.817577493702115*^9, 3.817577494190921*^9}, { + 3.81757759294232*^9, + 3.8175775932809963`*^9}},ExpressionUUID->"013ba19d-162c-4e14-9e6b-\ +43bcf125ceef"], + +Cell["\<\ +Information +- (1) None: Solution 1 (UDDU) / 1 (UDUD), Root t1 +- (2) None: Solution 1 (UDDU) / 1 (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}, {3.817558152473523*^9, 3.817558304739996*^9}, { + 3.817558375206586*^9, 3.8175583806143312`*^9}, {3.8175584286412487`*^9, + 3.817558429086872*^9}, {3.817561997387212*^9, + 3.817561998933936*^9}},ExpressionUUID->"ccf44efd-e09a-4bbb-a696-\ +b4c90f7a95eb"], Cell[CellGroupData[{ Cell[BoxData[{ + RowBox[{"tmp", "=", + RowBox[{"Simplify", "[", + RowBox[{"resultTa", "[", + RowBox[{"[", + RowBox[{"6", ",", "1", ",", "7", ",", "2"}], "]"}], "]"}], + "]"}]}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"coeffList", "=", - RowBox[{"Simplify", "[", - RowBox[{"CoefficientList", "[", - RowBox[{ - RowBox[{ - RowBox[{"(", - RowBox[{"resultTa", "[", - RowBox[{"[", - RowBox[{"2", ",", "2", ",", "3", ",", "2", ",", "1"}], "]"}], "]"}], - ")"}], "[", "x", "]"}], ",", "x"}], "]"}], "]"}]}], - ";"}], "\[IndentingNewLine]", - RowBox[{"ToString", "[", - RowBox[{ - RowBox[{"coeffList", "[", - RowBox[{"[", "1", "]"}], "]"}], ",", "CForm"}], - "]"}], "\[IndentingNewLine]", + RowBox[{"ToString", "[", + RowBox[{"tmp", ",", "CForm"}], "]"}], ";"}], "\[IndentingNewLine]", RowBox[{"CopyToClipboard", "[", "%", "]"}]}], "Input", - CellChangeTimes->{{3.817200459186548*^9, 3.817200516822897*^9}, { - 3.8172006139636374`*^9, 3.817200714738187*^9}, {3.817202163442213*^9, - 3.817202171636221*^9}, {3.81726802157551*^9, 3.817268079525145*^9}, { - 3.817268359134158*^9, 3.81726836240077*^9}}, + CellChangeTimes->{{3.817265499752473*^9, 3.817265502986458*^9}, { + 3.817265639391837*^9, 3.817265642449834*^9}, {3.81726571679879*^9, + 3.817265763955217*^9}, {3.8172658406982603`*^9, 3.8172658423822193`*^9}, { + 3.8172662344229*^9, 3.817266284843684*^9}, {3.817266331152025*^9, + 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}, {3.817558355798354*^9, + 3.817558365794586*^9}, {3.817558433307558*^9, 3.81755845142789*^9}, { + 3.817558538955283*^9, 3.8175585971878443`*^9}, {3.817558814172476*^9, + 3.817558823267172*^9}, {3.817558857798565*^9, 3.817558888772328*^9}, { + 3.817559063987196*^9, 3.817559086497924*^9}, {3.817559125299643*^9, + 3.817559148659762*^9}}, CellLabel-> - "In[608]:=",ExpressionUUID->"c24549d5-e55a-4ad4-a91d-4e532e8d2688"], + "In[357]:=",ExpressionUUID->"b4fd3303-7c30-4af8-94b1-4505577eaf40"], -Cell[BoxData["\<\"-Power(a0,6) + 6*Power(a0,5)*jMax*tf + \ -72*Power(jMax,3)*(jMax*Power(p0 - pf + tf*v0,2) + Power(v0 - vf,3)) - \ -6*Power(a0,4)*jMax*(2*jMax*Power(tf,2) - v0 + vf) + \ -24*Power(a0,3)*Power(jMax,2)*(2*p0 - 2*pf - tf*v0 + 3*tf*vf) + \ -72*a0*Power(jMax,3)*(jMax*Power(tf,2)*(p0 - pf + tf*v0) + (v0 - vf)*(-2*p0 + \ -2*pf + tf*v0 - 3*tf*vf)) + \ -18*Power(a0,2)*Power(jMax,2)*(Power(jMax,2)*Power(tf,4) - 2*Power(v0 - vf,2) \ -- 8*jMax*tf*(p0 - pf + tf*vf))\"\>"], "Output", - CellChangeTimes->{{3.817200687599572*^9, 3.817200715110696*^9}, { - 3.817202165192795*^9, 3.817202171958297*^9}, {3.8172680257100277`*^9, - 3.8172680798545218`*^9}, {3.817268359623165*^9, 3.817268362878442*^9}}, +Cell[BoxData[ + FractionBox[ + RowBox[{ + RowBox[{"-", "af"}], "+", "aMax"}], "jMax"]], "Output", + CellChangeTimes->{ + 3.817265503359898*^9, {3.817265635594121*^9, 3.817265642807448*^9}, { + 3.817265717144732*^9, 3.817265764334104*^9}, {3.8172658410199757`*^9, + 3.817265843063768*^9}, {3.817266228363278*^9, 3.817266285241193*^9}, + 3.817266336850995*^9, {3.817266389245998*^9, 3.817266447095387*^9}, { + 3.817266847014901*^9, 3.817266896239656*^9}, {3.817268768162199*^9, + 3.817268772746282*^9}, {3.817268819523574*^9, 3.8172688811055727`*^9}, { + 3.817269691950478*^9, 3.817269741332467*^9}, {3.817558435775181*^9, + 3.817558452639388*^9}, 3.817558503975954*^9, {3.8175585416431932`*^9, + 3.817558597702326*^9}, {3.8175588176540203`*^9, 3.8175588254753*^9}, { + 3.8175588614859667`*^9, 3.817558890483943*^9}, {3.817559065698791*^9, + 3.817559088471237*^9}, {3.817559128533183*^9, 3.8175591491556396`*^9}}, CellLabel-> - "Out[609]=",ExpressionUUID->"8a1b58c3-1441-4fe3-bbe7-d56171dd7cae"] + "Out[357]=",ExpressionUUID->"f535e841-b227-4186-840e-518e51e82b78"] }, Open ]], +Cell["Print roots", "Text", + CellChangeTimes->{{3.8172021457068987`*^9, 3.817202155527573*^9}, { + 3.817560821147089*^9, + 3.817560822280478*^9}},ExpressionUUID->"6e1bb727-a1dc-4c24-af7f-\ +abfa0f3b9409"], + Cell[CellGroupData[{ Cell[BoxData[{ - RowBox[{"tmp", "=", - RowBox[{ + RowBox[{ + RowBox[{"tRoot", "=", "1"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"tmpResult", "=", RowBox[{"resultTa", "[", RowBox[{"[", - RowBox[{"2", ",", "2", ",", "7", ",", "2"}], "]"}], "]"}], "//.", - RowBox[{"{", + RowBox[{"2", ",", "1"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"rootFunction", "=", + RowBox[{"tmpResult", "[", + RowBox[{"[", + RowBox[{"tRoot", ",", "2"}], "]"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{"tmp", "=", + RowBox[{"Simplify", "[", + FractionBox[ RowBox[{ - RowBox[{"resultTa", "[", - RowBox[{"[", - RowBox[{"2", ",", "2", ",", "3", ",", "2"}], "]"}], "]"}], "\[Rule]", - "t"}], "}"}]}]}], "\[IndentingNewLine]", - RowBox[{"ToString", "[", - RowBox[{ - RowBox[{"Simplify", "[", "tmp", "]"}], ",", "CForm"}], - "]"}], "\[IndentingNewLine]", + RowBox[{"CoefficientList", "[", + RowBox[{ + RowBox[{ + RowBox[{"rootFunction", "[", + RowBox[{"[", "1", "]"}], "]"}], "[", "x", "]"}], ",", "x"}], "]"}], + "[", + RowBox[{"[", "1", "]"}], "]"}], + RowBox[{"Last", "[", + RowBox[{"CoefficientList", "[", + RowBox[{ + RowBox[{ + RowBox[{"rootFunction", "[", + RowBox[{"[", "1", "]"}], "]"}], "[", "x", "]"}], ",", "x"}], "]"}], + "]"}]], "]"}]}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"ToString", "[", + RowBox[{"tmp", ",", "CForm"}], "]"}], ";"}], "\[IndentingNewLine]", RowBox[{"CopyToClipboard", "[", "%", "]"}]}], "Input", + CellChangeTimes->{{3.817560825638356*^9, 3.8175608257013817`*^9}, { + 3.817560859298699*^9, 3.817560859357276*^9}, {3.817560904953808*^9, + 3.817560911193849*^9}, {3.8175609549626417`*^9, 3.817560959584156*^9}, { + 3.817561108986223*^9, 3.817561109081521*^9}, {3.81756143478745*^9, + 3.817561455058796*^9}, {3.8175617151710873`*^9, 3.817561721322071*^9}, { + 3.817561767363587*^9, 3.817561794605496*^9}}, + CellLabel-> + "In[555]:=",ExpressionUUID->"b4d0a35e-2a8e-482f-b0a5-593ccb90a6dd"], + +Cell[BoxData[ + RowBox[{"-", + RowBox[{ + FractionBox["1", + RowBox[{"18", " ", + SuperscriptBox["jMax", "4"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"a0", "-", "af", "+", + RowBox[{"jMax", " ", "tf"}]}], ")"}], "2"]}]], + RowBox[{"(", + RowBox[{ + SuperscriptBox["a0", "6"], "+", + SuperscriptBox["af", "6"], "-", + RowBox[{"6", " ", + SuperscriptBox["a0", "5"], " ", + RowBox[{"(", + RowBox[{"af", "-", + RowBox[{"jMax", " ", "tf"}]}], ")"}]}], "+", + RowBox[{"48", " ", + SuperscriptBox["af", "3"], " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf", "+", + RowBox[{"tf", " ", "v0"}]}], ")"}]}], "-", + RowBox[{"72", " ", + SuperscriptBox["jMax", "3"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"jMax", " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"p0", "-", "pf", "+", + RowBox[{"tf", " ", "v0"}]}], ")"}], "2"]}], "-", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "3"]}], ")"}]}], "+", + RowBox[{"3", " ", + SuperscriptBox["a0", "4"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"5", " ", + SuperscriptBox["af", "2"]}], "-", + RowBox[{"8", " ", "af", " ", "jMax", " ", "tf"}], "+", + RowBox[{"2", " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "jMax", " ", + SuperscriptBox["tf", "2"]}], "+", "v0", "-", "vf"}], ")"}]}]}], + ")"}]}], "+", + RowBox[{"6", " ", + SuperscriptBox["af", "4"], " ", "jMax", " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}]}], "+", + RowBox[{"144", " ", "af", " ", + SuperscriptBox["jMax", "3"], " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf", "+", + RowBox[{"tf", " ", "v0"}]}], ")"}], " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}]}], "+", + RowBox[{"36", " ", + SuperscriptBox["af", "2"], " ", + SuperscriptBox["jMax", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"]}], "-", + RowBox[{"4", " ", + SuperscriptBox["a0", "3"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"5", " ", + SuperscriptBox["af", "3"]}], "-", + RowBox[{"9", " ", + SuperscriptBox["af", "2"], " ", "jMax", " ", "tf"}], "+", + RowBox[{"6", " ", "af", " ", "jMax", " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}]}], "+", + RowBox[{"6", " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "p0"}], "-", + RowBox[{"2", " ", "pf"}], "-", + RowBox[{"tf", " ", "v0"}], "+", + RowBox[{"3", " ", "tf", " ", "vf"}]}], ")"}]}]}], ")"}]}], "+", + RowBox[{"3", " ", + SuperscriptBox["a0", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"5", " ", + SuperscriptBox["af", "4"]}], "-", + RowBox[{"8", " ", + SuperscriptBox["af", "3"], " ", "jMax", " ", "tf"}], "-", + RowBox[{"12", " ", + SuperscriptBox["af", "2"], " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"jMax", " ", + SuperscriptBox["tf", "2"]}], "-", "v0", "+", "vf"}], ")"}]}], "+", + RowBox[{"24", " ", "af", " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "p0"}], "-", + RowBox[{"2", " ", "pf"}], "+", + RowBox[{"jMax", " ", + SuperscriptBox["tf", "3"]}], "+", + RowBox[{"2", " ", "tf", " ", "vf"}]}], ")"}]}], "-", + RowBox[{"6", " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + SuperscriptBox["jMax", "2"], " ", + SuperscriptBox["tf", "4"]}], "-", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"]}], "+", + RowBox[{"8", " ", "jMax", " ", "tf", " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf", "+", + RowBox[{"tf", " ", "vf"}]}], ")"}]}]}], ")"}]}]}], ")"}]}], "-", + RowBox[{"6", " ", "a0", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["af", "5"], "-", + RowBox[{ + SuperscriptBox["af", "4"], " ", "jMax", " ", "tf"}], "-", + RowBox[{"4", " ", + SuperscriptBox["af", "3"], " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"jMax", " ", + SuperscriptBox["tf", "2"]}], "-", "v0", "+", "vf"}], ")"}]}], "+", + RowBox[{"12", " ", + SuperscriptBox["af", "2"], " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "p0"}], "-", + RowBox[{"2", " ", "pf"}], "+", + RowBox[{"tf", " ", + RowBox[{"(", + RowBox[{"v0", "+", "vf"}], ")"}]}]}], ")"}]}], "-", + RowBox[{"12", " ", "af", " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"]}], "+", + RowBox[{"jMax", " ", "tf", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "p0"}], "-", + RowBox[{"2", " ", "pf"}], "+", + RowBox[{"3", " ", "tf", " ", "v0"}], "-", + RowBox[{"tf", " ", "vf"}]}], ")"}]}]}], ")"}]}], "+", + RowBox[{"12", " ", + SuperscriptBox["jMax", "3"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"jMax", " ", + SuperscriptBox["tf", "2"], " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf", "+", + RowBox[{"tf", " ", "v0"}]}], ")"}]}], "+", + RowBox[{ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "p0"}], "-", + RowBox[{"2", " ", "pf"}], "-", + RowBox[{"tf", " ", "v0"}], "+", + RowBox[{"3", " ", "tf", " ", "vf"}]}], ")"}]}]}], ")"}]}]}], + ")"}]}]}], ")"}]}]}]], "Output", CellChangeTimes->{ - 3.817202615037848*^9, {3.817202703455277*^9, 3.8172028329768457`*^9}, { - 3.817268093411096*^9, 3.817268131227783*^9}, {3.8175248932620068`*^9, - 3.8175248979557743`*^9}},ExpressionUUID->"27add3ed-17eb-49e9-9b82-\ -a98025fa047d"], + 3.817560826095455*^9, 3.817560859937997*^9, {3.817560911816668*^9, + 3.8175609601409273`*^9}, 3.817561109566786*^9, {3.817561439348381*^9, + 3.8175614555152063`*^9}, {3.817561718547414*^9, 3.817561721706765*^9}, { + 3.8175617609882193`*^9, 3.817561795479704*^9}}, + CellLabel-> + "Out[558]=",ExpressionUUID->"a0ebe065-b583-4fab-b2f2-0807a73eeffd"] +}, Open ]], + +Cell[CellGroupData[{ + +Cell[BoxData[{ + RowBox[{ + RowBox[{"tmpReplaced", "=", + RowBox[{"tmpResult", "//.", + RowBox[{"{", + RowBox[{"rootFunction", "\[Rule]", "t"}], "}"}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{"tmp", "=", + RowBox[{"FullSimplify", "[", + RowBox[{"tmpReplaced", "[", + RowBox[{"[", + RowBox[{"7", ",", "2"}], "]"}], "]"}], "]"}]}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"ToString", "[", + RowBox[{"tmp", ",", "CForm"}], "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{"CopyToClipboard", "[", "%", "]"}]}], "Input", + CellChangeTimes->{{3.817560846523941*^9, 3.817560846554605*^9}, { + 3.817560967186347*^9, 3.817561005993643*^9}, {3.8175611243144836`*^9, + 3.817561186536889*^9}, {3.8175618173664217`*^9, 3.817561891259448*^9}}, + CellLabel-> + "In[585]:=",ExpressionUUID->"111d8f11-4472-4521-8634-15b568e443ee"], -Cell[BoxData["\<\"(Power(a0,5) - Power(a0,4)*jMax*(3*t + 4*tf) + \ -Power(a0,3)*jMax*(jMax*(3*Power(t,2) + 12*t*tf + 5*Power(tf,2)) - 2*v0 + \ -2*vf) + 3*Power(a0,2)*Power(jMax,2)*(-14*p0 + 14*pf + 2*jMax*Power(t,3) - \ -3*jMax*Power(t,2)*tf - 5*jMax*t*Power(tf,2) + 2*jMax*Power(tf,3) + 2*t*v0 + \ -2*tf*v0 - 2*t*vf - 16*tf*vf) + 6*Power(jMax,3)*(Power(jMax,2)*t*Power(t - \ -tf,2)*Power(tf,2) - 2*(v0 - vf)*(-p0 + pf + tf*v0 - 2*tf*vf) + \ -jMax*tf*(6*pf*t + 2*pf*tf - 2*p0*(3*t + tf) - 3*Power(t,2)*v0 + t*tf*v0 - \ -2*Power(tf,2)*v0 + 3*Power(t,2)*vf - 7*t*tf*vf)) - \ -6*a0*Power(jMax,2)*(Power(jMax,2)*Power(t - tf,2)*tf*(2*t + tf) - 4*Power(v0 \ -- vf,2) - jMax*(6*p0*t - 6*pf*t + 10*p0*tf - 10*pf*tf + 3*Power(t,2)*v0 - \ -2*t*tf*v0 + 3*Power(tf,2)*v0 - 3*Power(t,2)*vf + 8*t*tf*vf + \ -7*Power(tf,2)*vf)))/(jMax*(Power(a0,4) - 4*Power(a0,3)*jMax*tf + \ -6*Power(a0,2)*Power(jMax,2)*Power(tf,2) - 24*a0*Power(jMax,2)*(p0 - pf + \ -tf*vf) + 12*Power(jMax,2)*(Power(v0 - vf,2) + jMax*tf*(2*p0 - 2*pf + tf*(v0 + \ -vf)))))\"\>"], "Output", +Cell[BoxData["0"], "Output", CellChangeTimes->{ - 3.817200666650454*^9, {3.817202710271187*^9, 3.817202833401093*^9}, { - 3.817268097335195*^9, 3.817268131943392*^9}}, + 3.817560846941321*^9, {3.817560925389645*^9, 3.8175610088048573`*^9}, { + 3.8175611134866962`*^9, 3.8175611925592327`*^9}, {3.817561817744227*^9, + 3.817561891480846*^9}}, CellLabel-> - "Out[601]=",ExpressionUUID->"9edd45ed-0ce4-41cc-9909-2527fddc394d"] + "Out[586]=",ExpressionUUID->"d447ada5-9a92-4205-84f4-0b4ec8cacc6f"] }, Open ]], Cell[CellGroupData[{ @@ -1487,6 +2583,68 @@ Cell[BoxData[ 3.817532801189687*^9}, 3.817532948179289*^9}, CellLabel-> "Out[194]=",ExpressionUUID->"f8a67d4a-9073-4868-948b-275b14bd35f8"] +}, Open ]], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{ + RowBox[{"af", "\[Equal]", + RowBox[{"a0", "+", + RowBox[{"t1", "*", "jMax"}]}]}], ",", + RowBox[{"vf", "\[Equal]", + RowBox[{"v0", "+", + RowBox[{"t1", "*", "a0"}], "+", + RowBox[{ + FractionBox["1", "2"], "jMax", " ", + SuperscriptBox["t1", "2"]}]}]}]}], "}"}]], "Input", + CellChangeTimes->{{3.8176118175766582`*^9, + 3.817611850967111*^9}},ExpressionUUID->"ac4e2179-0486-40d4-a93e-\ +0bd772b55249"], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{ + RowBox[{ + RowBox[{ + RowBox[{"a0", "+", + RowBox[{"jMax", "*", "t1"}]}], "/.", + RowBox[{"Solve", "[", + RowBox[{ + RowBox[{"vf", "\[Equal]", + RowBox[{"v0", "+", + RowBox[{"t1", "*", "a0"}], "+", + RowBox[{ + FractionBox["1", "2"], "jMax", " ", + SuperscriptBox["t1", "2"]}]}]}], ",", "t1"}], "]"}]}], "/.", + RowBox[{"{", + RowBox[{"v0", "\[Rule]", + RowBox[{"-", "vMax"}]}], "}"}]}], "/.", + RowBox[{"{", + RowBox[{"a0", "\[Rule]", "0"}], "}"}]}]], "Input", + CellChangeTimes->{{3.81761188312281*^9, 3.8176118938899527`*^9}, { + 3.817611948557341*^9, 3.8176119526979847`*^9}, {3.817612002826972*^9, + 3.81761204772519*^9}, {3.817612133148159*^9, 3.817612149326852*^9}, { + 3.81761222810284*^9, 3.817612325789792*^9}}, + CellLabel-> + "In[916]:=",ExpressionUUID->"5e6a25cd-7819-4650-a7d0-08f771745532"], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{ + SqrtBox[ + RowBox[{ + RowBox[{"2", " ", "jMax", " ", "vf"}], "+", + RowBox[{"2", " ", "jMax", " ", "vMax"}]}]], ",", + RowBox[{"-", + SqrtBox[ + RowBox[{ + RowBox[{"2", " ", "jMax", " ", "vf"}], "+", + RowBox[{"2", " ", "jMax", " ", "vMax"}]}]]}]}], "}"}]], "Output", + CellChangeTimes->{{3.8176121441258698`*^9, 3.817612149837326*^9}, { + 3.817612234521276*^9, 3.8176122953597183`*^9}, 3.817612326482416*^9}, + CellLabel-> + "Out[916]=",ExpressionUUID->"3d5e1dff-06d5-4a78-8211-d677f7d95c06"] }, Open ]] }, WindowSize->{904, 920}, @@ -1508,46 +2666,51 @@ CellTagsIndex->{} (*NotebookFileOutline Notebook[{ Cell[558, 20, 230, 4, 31, "Text",ExpressionUUID->"039b787c-64fd-4dfb-8011-578c383c402e"], -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[791, 26, 7746, 237, 822, "Input",ExpressionUUID->"9382707b-2876-460d-b0fb-ca70e05b1e9a"], +Cell[8540, 265, 13311, 343, 446, "Input",ExpressionUUID->"93a49805-7b39-4e71-a5ce-78f2b77ae9c0"], +Cell[21854, 610, 1309, 26, 238, "Text",ExpressionUUID->"0616ae41-3a02-473a-878f-96412e2fc8c5"], +Cell[23166, 638, 284, 7, 52, "Text",ExpressionUUID->"407acd61-eed1-4280-b150-0da33c32906a"], +Cell[CellGroupData[{ +Cell[23475, 649, 935, 22, 66, "Input",ExpressionUUID->"f08feb2e-6079-473b-b866-92670dea0a56"], +Cell[24413, 673, 5903, 142, 154, "Output",ExpressionUUID->"faa343c3-02f2-4fc4-b2b0-2208fa60cf35"] +}, Open ]], +Cell[30331, 818, 247, 4, 31, "Text",ExpressionUUID->"f1684154-b22e-4827-98da-4e4dad150835"], Cell[CellGroupData[{ -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"] +Cell[30603, 826, 1854, 46, 144, "Input",ExpressionUUID->"217d7885-1018-483e-8952-08082feb5695"], +Cell[32460, 874, 4300, 116, 141, "Output",ExpressionUUID->"73320609-b23d-4237-841c-1809a13b55cc"] }, Open ]], -Cell[30069, 815, 247, 4, 31, "Text",ExpressionUUID->"f1684154-b22e-4827-98da-4e4dad150835"], Cell[CellGroupData[{ -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"] +Cell[36797, 995, 1131, 24, 85, "Input",ExpressionUUID->"58c30e25-83c4-4d05-ad21-e88fcb7f4181"], +Cell[37931, 1021, 32291, 849, 844, "Output",ExpressionUUID->"db1e6e47-7503-4b17-aaaf-3bbf9d9ffecb"] }, Open ]], +Cell[70237, 1873, 229, 4, 31, "Text",ExpressionUUID->"5c00d19c-efbb-475f-a4a4-adc1877059e7"], +Cell[70469, 1879, 5732, 131, 275, "Input",ExpressionUUID->"013ba19d-162c-4e14-9e6b-43bcf125ceef"], +Cell[76204, 2012, 869, 20, 218, "Text",ExpressionUUID->"ccf44efd-e09a-4bbb-a696-b4c90f7a95eb"], Cell[CellGroupData[{ -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"] +Cell[77098, 2036, 1355, 25, 66, "Input",ExpressionUUID->"b4fd3303-7c30-4af8-94b1-4505577eaf40"], +Cell[78456, 2063, 1009, 17, 50, "Output",ExpressionUUID->"f535e841-b227-4186-840e-518e51e82b78"] }, Open ]], -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[79480, 2083, 204, 4, 31, "Text",ExpressionUUID->"6e1bb727-a1dc-4c24-af7f-abfa0f3b9409"], Cell[CellGroupData[{ -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"] +Cell[79709, 2091, 1650, 43, 144, "Input",ExpressionUUID->"b4d0a35e-2a8e-482f-b0a5-593ccb90a6dd"], +Cell[81362, 2136, 6824, 186, 207, "Output",ExpressionUUID->"a0ebe065-b583-4fab-b2f2-0807a73eeffd"] }, Open ]], -Cell[44210, 1167, 182, 3, 31, "Text",ExpressionUUID->"6e1bb727-a1dc-4c24-af7f-abfa0f3b9409"], Cell[CellGroupData[{ -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"] +Cell[88223, 2327, 841, 20, 85, "Input",ExpressionUUID->"111d8f11-4472-4521-8634-15b568e443ee"], +Cell[89067, 2349, 308, 6, 31, "Output",ExpressionUUID->"d447ada5-9a92-4205-84f4-0b4ec8cacc6f"] }, Open ]], Cell[CellGroupData[{ -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"] +Cell[89412, 2360, 1812, 44, 156, "Input",ExpressionUUID->"0d5d8425-f226-4e6a-91fd-34a5822f73b9"], +Cell[91227, 2406, 2994, 80, 155, "Output",ExpressionUUID->"1dcd2096-117e-4fa7-8a27-82d21901823d"] }, Open ]], Cell[CellGroupData[{ -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"] +Cell[94258, 2491, 764, 21, 46, "Input",ExpressionUUID->"4f3c940a-b41f-43f3-9aa8-8115c96b6bee"], +Cell[95025, 2514, 2830, 70, 76, "Output",ExpressionUUID->"f8a67d4a-9073-4868-948b-275b14bd35f8"] }, Open ]], +Cell[97870, 2587, 472, 14, 42, "Input",ExpressionUUID->"ac4e2179-0486-40d4-a93e-0bd772b55249"], Cell[CellGroupData[{ -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"] +Cell[98367, 2605, 895, 24, 42, "Input",ExpressionUUID->"5e6a25cd-7819-4650-a7d0-08f771745532"], +Cell[99265, 2631, 561, 15, 59, "Output",ExpressionUUID->"3d5e1dff-06d5-4a78-8211-d677f7d95c06"] }, Open ]] } ] diff --git a/src/profile.cpp b/src/profile.cpp index a7ac4203..306ee27f 100644 --- a/src/profile.cpp +++ b/src/profile.cpp @@ -24,7 +24,7 @@ void Profile::set(double p0, double v0, double a0, const std::array& bool Profile::check(double pf, double vf, double af, double vMax, double aMax) const { // Velocity and acceleration limits can be broken in the beginning if the initial velocity and acceleration are too high - // std::cout << std::setprecision(15) << "target: " << std::abs(p[7]-pf) << " " << std::abs(v[7] - vf) << std::endl; + // std::cout << std::setprecision(15) << "target: " << std::abs(p[7]-pf) << " " << std::abs(v[7] - vf) << " " << std::abs(a[7] - af) << std::endl; return std::all_of(t.begin(), t.end(), [](double tm){ return tm >= 0; }) && std::all_of(v.begin() + 3, v.end(), [vMax](double vm){ return std::abs(vm) < std::abs(vMax) + 1e-9; }) && std::all_of(a.begin() + 2, a.end(), [aMax](double am){ return std::abs(am) < std::abs(aMax) + 1e-9; }) @@ -33,7 +33,7 @@ bool Profile::check(double pf, double vf, double af, double vMax, double aMax) c bool Profile::check(double tf, double pf, double vf, double af, double vMax, double aMax) const { // std::cout << std::setprecision(15) << "target: " << std::abs(t_sum[6]-tf) << " " << std::abs(p[7]-pf) << " " << std::abs(v[7] - vf) << std::endl; - return std::abs(t_sum[6] - tf) < 3e-3 && check(pf, vf, af, vMax, aMax); + return std::abs(t_sum[6] - tf) < 2e-3 && check(pf, vf, af, vMax, aMax); } std::tuple Profile::integrate(double t, double p0, double v0, double a0, double j) { diff --git a/src/step1.cpp b/src/step1.cpp index 99856bee..c89ddf40 100644 --- a/src/step1.cpp +++ b/src/step1.cpp @@ -174,6 +174,44 @@ bool RuckigStep1::time_up_acc0(Profile& profile, double vMax, double aMax, doubl return true; } } + + // UDUD + /* if (std::abs(af) > DBL_EPSILON) { + std::array polynom; + polynom[0] = 1.0; + polynom[1] = (-2*aMax)/jMax; + polynom[2] = -((Power(af,2) + Power(aMax,2) + 2*jMax*vf)/Power(jMax,2)); + polynom[3] = (2*aMax*(Power(af,2) + 2*jMax*vf))/Power(jMax,3); + polynom[4] = (-3*Power(a0,4) + 3*Power(af,4) + 8*Power(a0,3)*aMax - 8*Power(af,3)*aMax - 24*a0*aMax*jMax*v0 - 6*Power(a0,2)*(Power(aMax,2) - 2*jMax*v0) - 24*af*aMax*jMax*vf + 6*Power(af,2)*(Power(aMax,2) + 2*jMax*vf) + 12*jMax*(2*aMax*jMax*(p0 - pf) + Power(aMax,2)*(v0 + vf) + jMax*(-Power(v0,2) + Power(vf,2))))/(12.*Power(jMax,4)); + + auto roots = Roots::solveQuart(polynom); + for (double t: roots) { + if (t <= 0.0) { + continue; + } + + profile.t[0] = (-a0 + aMax)/jMax; + profile.t[1] = (Power(a0,2) + Power(af,2) - 2*(Power(aMax,2) + jMax*(jMax*Power(t,2) + v0 - vf)))/(2.*aMax*jMax); + profile.t[2] = aMax/jMax; + profile.t[3] = 0; + profile.t[4] = t; + profile.t[5] = 0; + profile.t[6] = -(af/jMax) + t; + + 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; + + profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, jMax, 0, -jMax}); + if (profile.check(pf, vf, af, vMax, aMax)) { + return true; + } + } + } */ return false; } @@ -225,6 +263,89 @@ bool RuckigStep1::time_up_none(Profile& profile, double vMax, double aMax, doubl return true; } } + + // UDUD + if (std::abs(af) > DBL_EPSILON) { + std::array polynom; + polynom[0] = 1.0; + polynom[1] = (6*a0)/jMax; + polynom[2] = (53*Power(a0,2) + Power(af,2) + 2*jMax*(7*v0 + vf))/(4.*Power(jMax,2)); + polynom[3] = (40*Power(a0,3) - Power(af,3) + 3*Power(jMax,2)*(p0 - pf) - 3*af*jMax*vf + 3*a0*(Power(af,2) + 13*jMax*v0 + 2*jMax*vf))/(3.*Power(jMax,3)); + polynom[4] = (51*Power(a0,4) - Power(af,4) + 4*Power(af,2)*jMax*(v0 - vf) + 2*Power(a0,2)*(5*Power(af,2) + 58*jMax*v0 + 10*jMax*vf) - 8*a0*(Power(af,3) + 3*Power(jMax,2)*(-p0 + pf) + 3*af*jMax*vf) + 4*Power(jMax,2)*(7*Power(v0,2) + 2*v0*vf - Power(vf,2)))/(8.*Power(jMax,4)); + polynom[5] = (17*Power(a0,5) + 2*Power(a0,3)*(3*Power(af,2) + 34*jMax*v0 + 6*jMax*vf) - 8*Power(a0,2)*(Power(af,3) + 3*Power(jMax,2)*(-p0 + pf) + 3*af*jMax*vf) - 8*jMax*v0*(Power(af,3) + 3*Power(jMax,2)*(-p0 + pf) + 3*af*jMax*vf) - 3*a0*(Power(af,4) + 4*Power(af,2)*jMax*(-v0 + vf) + 4*Power(jMax,2)*(-5*Power(v0,2) - 2*v0*vf + Power(vf,2))))/(12.*Power(jMax,5)); + polynom[6] = -(-17*Power(a0,6) + Power(af,6) + 48*Power(af,3)*Power(jMax,2)*(p0 - pf) + 144*af*Power(jMax,3)*(p0 - pf)*vf + 6*Power(af,4)*jMax*(3*v0 + vf) - 3*Power(a0,4)*(3*Power(af,2) + 34*jMax*v0 + 6*jMax*vf) + 16*Power(a0,3)*(Power(af,3) + 3*Power(jMax,2)*(-p0 + pf) + 3*af*jMax*vf) + 48*a0*jMax*v0*(Power(af,3) + 3*Power(jMax,2)*(-p0 + pf) + 3*af*jMax*vf) - 36*Power(af,2)*Power(jMax,2)*(Power(v0,2) - 2*v0*vf - Power(vf,2)) - 72*Power(jMax,3)*(jMax*Power(p0 - pf,2) + (v0 - vf)*Power(v0 + vf,2)) + 9*Power(a0,2)*(Power(af,4) + 4*Power(af,2)*jMax*(-v0 + vf) + 4*Power(jMax,2)*(-5*Power(v0,2) - 2*v0*vf + Power(vf,2))))/(144.*Power(jMax,6)); + + std::array deriv; + deriv[0] = 6 * polynom[0]; + deriv[1] = 5 * polynom[1]; + deriv[2] = 4 * polynom[2]; + deriv[3] = 3 * polynom[3]; + deriv[4] = 2 * polynom[4]; + deriv[5] = polynom[5]; + + auto dd_extremas = Roots::solveQuart(5 * deriv[0], 4 * deriv[1], 3 * deriv[2], 2 * deriv[3], deriv[4]); + std::set> dd_tz_intervals; + + double tz_min {0.0}; + double tz_max = {1000.0}; + double dd_tz_current {tz_min}; + + for (double tz: dd_extremas) { + 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 (Roots::polyEval(deriv, dd_tz_current) * Roots::polyEval(deriv, tz) < 0) { + dd_tz_intervals.insert({dd_tz_current, tz}); + } + dd_tz_current = tz; + } + if (Roots::polyEval(deriv, dd_tz_current) * Roots::polyEval(deriv, tz_max) < 0) { + dd_tz_intervals.insert({dd_tz_current, tz_max}); + } + + std::set> tz_intervals; + 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, 1e-14); + + 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}); + } + tz_current = tz; + } + if (Roots::polyEval(polynom, tz_current) * Roots::polyEval(polynom, tz_max) < 0) { + tz_intervals.insert({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, 1e-14); + + profile.t[0] = t; + profile.t[1] = 0; + profile.t[2] = a0/jMax + t; + profile.t[3] = 0; + profile.t[4] = -((29*Power(a0,7) + 12*Power(af,7) + 245*Power(a0,6)*jMax*t - Power(af,6)*jMax*t - 12*Power(af,5)*jMax*(jMax*Power(t,2) + 2*v0 - 7*vf) - 6*Power(af,4)*Power(jMax,2)*(6*p0 - 6*pf + t*(3*jMax*Power(t,2) + 3*v0 + vf)) + 3*Power(a0,5)*(3*Power(af,2) + 2*jMax*(117*jMax*Power(t,2) + 7*v0 + 3*vf)) + 36*Power(af,2)*Power(jMax,3)*(2*Power(jMax,2)*Power(t,5) - 2*pf*v0 + 5*t*Power(v0,2) + 2*p0*(v0 - 2*vf) + 4*pf*vf - 2*t*v0*vf - t*Power(vf,2) + jMax*Power(t,2)*(p0 - pf + 6*t*v0 - 2*t*vf)) - 72*af*Power(jMax,3)*vf*(2*Power(jMax,2)*Power(t,4) + 2*(v0 - vf)*vf + jMax*t*(2*p0 - 2*pf + 3*t*v0 + t*vf)) - 12*Power(af,3)*Power(jMax,2)*(4*Power(jMax,2)*Power(t,4) + 2*(5*v0 - 8*vf)*vf + jMax*t*(4*p0 - 4*pf + 6*t*v0 + 5*t*vf)) + Power(a0,4)*(-28*Power(af,3) + 105*Power(af,2)*jMax*t - 84*af*jMax*vf + 6*Power(jMax,2)*(14*p0 - 14*pf + 137*jMax*Power(t,3) - 11*t*v0 + 35*t*vf)) - 3*Power(a0,2)*jMax*(15*Power(af,4)*t + Power(af,3)*(84*jMax*Power(t,2) + 8*v0) - 12*Power(af,2)*jMax*t*(17*jMax*Power(t,2) + 11*v0 - 5*vf) + 12*af*jMax*(21*jMax*Power(t,2) + 2*v0)*vf - 12*Power(jMax,2)*(2*Power(jMax,2)*Power(t,5) + 2*p0*v0 - 2*pf*v0 - 31*t*Power(v0,2) + 22*t*v0*vf - 5*t*Power(vf,2) + jMax*Power(t,2)*(21*p0 - 21*pf - 46*t*v0 + 34*t*vf))) - Power(a0,3)*(21*Power(af,4) + 136*Power(af,3)*jMax*t - 12*Power(af,2)*jMax*(34*jMax*Power(t,2) + 5*v0 - 7*vf) + 408*af*Power(jMax,2)*t*vf - 12*Power(jMax,2)*(34*Power(jMax,2)*Power(t,4) - 15*Power(v0,2) + 10*v0*vf - 7*Power(vf,2) + jMax*t*(34*p0 - 34*pf - 87*t*v0 + 68*t*vf))) + 72*Power(jMax,4)*(2*Power(jMax,2)*Power(t,4)*(p0 - pf + t*(-v0 + vf)) - (v0 - vf)*(2*(-p0 + pf)*vf + t*(3*Power(v0,2) - 2*v0*vf - Power(vf,2))) + jMax*t*(Power(p0,2) - 2*p0*pf + Power(pf,2) + p0*t*(3*v0 + vf) - pf*t*(3*v0 + vf) - Power(t,2)*(5*Power(v0,2) - 6*v0*vf + Power(vf,2)))) - a0*(Power(af,6) + 24*Power(af,5)*jMax*t + 6*Power(af,4)*jMax*(9*jMax*Power(t,2) - 3*v0 + vf) + 24*Power(af,3)*Power(jMax,2)*(2*p0 - 2*pf + 8*jMax*Power(t,3) + 4*t*v0 + 5*t*vf) + 144*af*Power(jMax,3)*vf*(p0 - pf + t*(4*jMax*Power(t,2) + 2*v0 + vf)) - 36*Power(af,2)*Power(jMax,2)*(10*Power(jMax,2)*Power(t,4) + 3*Power(v0,2) + 2*v0*vf - Power(vf,2) + jMax*t*(2*p0 - 2*pf + 17*t*v0 - 6*t*vf)) - 72*Power(jMax,3)*(-3*Power(v0,3) + 3*Power(v0,2)*vf + v0*Power(vf,2) - Power(vf,3) + 2*Power(jMax,2)*Power(t,3)*(4*p0 - 4*pf - 6*t*v0 + 5*t*vf) + jMax*(Power(p0,2) - 2*p0*pf + Power(pf,2) + 2*p0*t*(2*v0 + vf) - 2*pf*t*(2*v0 + vf) + Power(t,2)*(-18*Power(v0,2) + 17*v0*vf - 3*Power(vf,2))))))/(jMax*(Power(a0,6) - 17*Power(af,6) + 48*Power(af,3)*Power(jMax,2)*(p0 - pf) + 6*Power(af,4)*jMax*(3*v0 - 17*vf) + 144*af*Power(jMax,3)*(p0 - pf)*vf + 3*Power(a0,4)*(3*Power(af,2) - 2*jMax*v0 + 6*jMax*vf) + 16*Power(a0,3)*(Power(af,3) + 3*Power(jMax,2)*(-p0 + pf) + 3*af*jMax*vf) - 48*a0*jMax*v0*(Power(af,3) + 3*Power(jMax,2)*(-p0 + pf) + 3*af*jMax*vf) + 36*Power(af,2)*Power(jMax,2)*(Power(v0,2) + 2*v0*vf - 5*Power(vf,2)) - 72*Power(jMax,3)*(jMax*Power(p0 - pf,2) + Power(v0 - vf,2)*(v0 + vf)) - 9*Power(a0,2)*(Power(af,4) + 4*Power(af,2)*jMax*(v0 + vf) + 4*Power(jMax,2)*(-Power(v0,2) + 2*v0*vf + Power(vf,2)))))); + profile.t[5] = 0; + profile.t[6] = -((29*Power(a0,7) - 5*Power(af,7) - Power(af,6)*jMax*t + Power(a0,6)*(af + 245*jMax*t) - 6*Power(af,5)*jMax*(2*jMax*Power(t,2) + v0 + 3*vf) - 6*Power(af,4)*Power(jMax,2)*(-2*p0 + 2*pf + t*(3*jMax*Power(t,2) + 3*v0 + vf)) + 3*Power(a0,5)*(3*Power(af,2) + 2*jMax*(117*jMax*Power(t,2) + 7*v0 + 3*vf)) + 36*Power(af,2)*Power(jMax,3)*(2*Power(jMax,2)*Power(t,5) + 2*p0*v0 - 2*pf*v0 + 5*t*Power(v0,2) - 2*t*v0*vf - t*Power(vf,2) + jMax*Power(t,2)*(p0 - pf + 6*t*v0 - 2*t*vf)) - 12*Power(af,3)*Power(jMax,2)*(4*Power(jMax,2)*Power(t,4) - 3*Power(v0,2) + 4*v0*vf - Power(vf,2) + jMax*t*(4*p0 - 4*pf + 6*t*v0 + 5*t*vf)) + Power(a0,4)*(-19*Power(af,3) + 105*Power(af,2)*jMax*t - 6*af*jMax*(v0 + 11*vf) + 6*Power(jMax,2)*(14*p0 - 14*pf + 137*jMax*Power(t,3) - 11*t*v0 + 35*t*vf)) - 72*af*Power(jMax,3)*(Power(v0,3) + 2*Power(jMax,2)*Power(t,4)*vf - Power(v0,2)*vf + v0*Power(vf,2) - Power(vf,3) + jMax*(Power(p0,2) + Power(pf,2) - 2*pf*t*vf + Power(t,2)*vf*(3*v0 + vf) - 2*p0*(pf - t*vf))) - 3*Power(a0,2)*(3*Power(af,5) + 15*Power(af,4)*jMax*t - 12*Power(af,2)*Power(jMax,2)*t*(17*jMax*Power(t,2) + 11*v0 - 5*vf) + 4*Power(af,3)*jMax*(21*jMax*Power(t,2) + 5*v0 + 3*vf) + 12*af*Power(jMax,2)*(-Power(v0,2) + 4*v0*vf + vf*(21*jMax*Power(t,2) + vf)) - 12*Power(jMax,3)*(2*Power(jMax,2)*Power(t,5) + 2*p0*v0 - 2*pf*v0 - 31*t*Power(v0,2) + 22*t*v0*vf - 5*t*Power(vf,2) + jMax*Power(t,2)*(21*p0 - 21*pf - 46*t*v0 + 34*t*vf))) - Power(a0,3)*(5*Power(af,4) + 136*Power(af,3)*jMax*t - 12*Power(af,2)*jMax*(34*jMax*Power(t,2) + 5*v0 - 3*vf) + 24*af*Power(jMax,2)*(2*p0 - 2*pf + 17*t*vf) - 12*Power(jMax,2)*(34*Power(jMax,2)*Power(t,4) - 15*Power(v0,2) + 10*v0*vf - 7*Power(vf,2) + jMax*t*(34*p0 - 34*pf - 87*t*v0 + 68*t*vf))) + 72*Power(jMax,4)*(2*Power(jMax,2)*Power(t,4)*(p0 - pf + t*(-v0 + vf)) - (v0 - vf)*(2*(-p0 + pf)*vf + t*(3*Power(v0,2) - 2*v0*vf - Power(vf,2))) + jMax*t*(Power(p0,2) - 2*p0*pf + Power(pf,2) + p0*t*(3*v0 + vf) - pf*t*(3*v0 + vf) - Power(t,2)*(5*Power(v0,2) - 6*v0*vf + Power(vf,2)))) - a0*(Power(af,6) + 24*Power(af,5)*jMax*t + 6*Power(af,4)*jMax*(9*jMax*Power(t,2) + 5*v0 + vf) + 24*Power(af,3)*Power(jMax,2)*(2*p0 - 2*pf + 8*jMax*Power(t,3) + 4*t*v0 + 5*t*vf) + 144*af*Power(jMax,3)*(pf*(v0 - vf) + p0*(-v0 + vf) + t*vf*(4*jMax*Power(t,2) + 2*v0 + vf)) - 36*Power(af,2)*Power(jMax,2)*(10*Power(jMax,2)*Power(t,4) + 3*Power(v0,2) - 2*v0*vf - Power(vf,2) + jMax*t*(2*p0 - 2*pf + 17*t*v0 - 6*t*vf)) - 72*Power(jMax,3)*(-3*Power(v0,3) + 3*Power(v0,2)*vf + v0*Power(vf,2) - Power(vf,3) + 2*Power(jMax,2)*Power(t,3)*(4*p0 - 4*pf - 6*t*v0 + 5*t*vf) + jMax*(Power(p0,2) - 2*p0*pf + Power(pf,2) + 2*p0*t*(2*v0 + vf) - 2*pf*t*(2*v0 + vf) + Power(t,2)*(-18*Power(v0,2) + 17*v0*vf - 3*Power(vf,2))))))/(jMax*(Power(a0,6) - 17*Power(af,6) + 48*Power(af,3)*Power(jMax,2)*(p0 - pf) + 6*Power(af,4)*jMax*(3*v0 - 17*vf) + 144*af*Power(jMax,3)*(p0 - pf)*vf + 3*Power(a0,4)*(3*Power(af,2) - 2*jMax*v0 + 6*jMax*vf) + 16*Power(a0,3)*(Power(af,3) + 3*Power(jMax,2)*(-p0 + pf) + 3*af*jMax*vf) - 48*a0*jMax*v0*(Power(af,3) + 3*Power(jMax,2)*(-p0 + pf) + 3*af*jMax*vf) + 36*Power(af,2)*Power(jMax,2)*(Power(v0,2) + 2*v0*vf - 5*Power(vf,2)) - 72*Power(jMax,3)*(jMax*Power(p0 - pf,2) + Power(v0 - vf,2)*(v0 + vf)) - 9*Power(a0,2)*(Power(af,4) + 4*Power(af,2)*jMax*(v0 + vf) + 4*Power(jMax,2)*(-Power(v0,2) + 2*v0*vf + Power(vf,2)))))); + + profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, jMax, 0, -jMax}); + if (profile.check(pf, vf, af, vMax, aMax)) { + return true; + } + } + } return false; } diff --git a/src/step2.cpp b/src/step2.cpp index af37fa65..cfae4aaf 100644 --- a/src/step2.cpp +++ b/src/step2.cpp @@ -166,11 +166,11 @@ bool RuckigStep2::time_up_vel(Profile& profile, double vMax, double aMax, double // Find root of 5th order polynom std::array polynom; polynom[0] = 1.0; - polynom[1] = (15*Power(a0,2) + 16*a0*jMax*tf - 2*Power(jMax,2)*Power(tf,2) + 6*jMax*v0 - 6*jMax*vf)/(4*a0*jMax + 4*Power(jMax,2)*tf); - polynom[2] = (29*Power(a0,3) + 33*Power(a0,2)*jMax*tf + 6*Power(jMax,2)*(p0 - pf + tf*v0) - 12*a0*jMax*(jMax*Power(tf,2) - 3*v0 + 3*vf))/(6.*Power(jMax,2)*(a0 + jMax*tf)); - polynom[3] = (61*Power(a0,4) + 76*Power(a0,3)*jMax*tf + 48*a0*Power(jMax,2)*(p0 - pf + tf*v0) - 24*Power(jMax,3)*tf*(p0 - pf + tf*v0) + 36*Power(jMax,2)*Power(v0 - vf,2) - 60*Power(a0,2)*jMax*(jMax*Power(tf,2) - 3*v0 + 3*vf))/(24.*Power(jMax,3)*(a0 + jMax*tf)); - polynom[4] = (a0*(7*Power(a0,4) + 10*Power(a0,3)*jMax*tf + 12*a0*Power(jMax,2)*(p0 - pf + tf*v0) - 24*Power(jMax,3)*tf*(p0 - pf + tf*v0) + 36*Power(jMax,2)*Power(v0 - vf,2) - 12*Power(a0,2)*jMax*(jMax*Power(tf,2) - 3*v0 + 3*vf)))/(12.*Power(jMax,4)*(a0 + jMax*tf)); - polynom[5] = (7*Power(a0,6) + 12*Power(a0,5)*jMax*tf + 24*Power(a0,3)*Power(jMax,2)*(p0 - pf + tf*v0) - 36*Power(a0,2)*Power(jMax,2)*(2*jMax*tf*(p0 - pf + tf*v0) - 3*Power(v0 - vf,2)) - 72*Power(jMax,3)*(jMax*Power(p0 - pf + tf*v0,2) - Power(v0 - vf,3)) - 18*Power(a0,4)*jMax*(jMax*Power(tf,2) - 3*v0 + 3*vf))/(144.*Power(jMax,5)*(a0 + jMax*tf)); + polynom[1] = (15*Power(a0,2) + Power(af,2) + 4*af*jMax*tf - 16*a0*(af - jMax*tf) - 2*jMax*(jMax*Power(tf,2) - 3*v0 + 3*vf))/(4.*jMax*(a0 - af + jMax*tf)); + polynom[2] = (29*Power(a0,3) - 2*Power(af,3) - 33*Power(a0,2)*(af - jMax*tf) + 6*Power(jMax,2)*(p0 - pf + tf*v0) + 6*af*jMax*(-v0 + vf) + 6*a0*(Power(af,2) + 4*af*jMax*tf - 2*jMax*(jMax*Power(tf,2) - 3*v0 + 3*vf)))/(6.*Power(jMax,2)*(a0 - af + jMax*tf)); + polynom[3] = (61*Power(a0,4) + Power(af,4) + 8*Power(af,3)*jMax*tf - 76*Power(a0,3)*(af - jMax*tf) - 24*Power(jMax,3)*tf*(p0 - pf + tf*v0) - 16*a0*(Power(af,3) - 3*Power(jMax,2)*(p0 - pf + tf*v0) + 3*af*jMax*(v0 - vf)) + 12*Power(af,2)*jMax*(v0 - vf) + 36*Power(jMax,2)*Power(v0 - vf,2) + 24*af*Power(jMax,2)*(p0 - pf + 2*tf*v0 - tf*vf) + 30*Power(a0,2)*(Power(af,2) + 4*af*jMax*tf - 2*jMax*(jMax*Power(tf,2) - 3*v0 + 3*vf)))/(24.*Power(jMax,3)*(a0 - af + jMax*tf)); + polynom[4] = (a0*(7*Power(a0,4) + Power(af,4) + 8*Power(af,3)*jMax*tf - 10*Power(a0,3)*(af - jMax*tf) - 24*Power(jMax,3)*tf*(p0 - pf + tf*v0) - 4*a0*(Power(af,3) - 3*Power(jMax,2)*(p0 - pf + tf*v0) + 3*af*jMax*(v0 - vf)) + 12*Power(af,2)*jMax*(v0 - vf) + 36*Power(jMax,2)*Power(v0 - vf,2) + 24*af*Power(jMax,2)*(p0 - pf + 2*tf*v0 - tf*vf) + 6*Power(a0,2)*(Power(af,2) + 4*af*jMax*tf - 2*jMax*(jMax*Power(tf,2) - 3*v0 + 3*vf))))/(12.*Power(jMax,4)*(a0 - af + jMax*tf)); + polynom[5] = (7*Power(a0,6) + Power(af,6) - 12*Power(a0,5)*(af - jMax*tf) + 48*Power(af,3)*Power(jMax,2)*(p0 - pf + tf*v0) - 8*Power(a0,3)*(Power(af,3) - 3*Power(jMax,2)*(p0 - pf + tf*v0) + 3*af*jMax*(v0 - vf)) - 72*Power(jMax,3)*(jMax*Power(p0 - pf + tf*v0,2) - Power(v0 - vf,3)) + 6*Power(af,4)*jMax*(v0 - vf) + 144*af*Power(jMax,3)*(p0 - pf + tf*v0)*(v0 - vf) + 36*Power(af,2)*Power(jMax,2)*Power(v0 - vf,2) + 9*Power(a0,4)*(Power(af,2) + 4*af*jMax*tf - 2*jMax*(jMax*Power(tf,2) - 3*v0 + 3*vf)) + 3*Power(a0,2)*(Power(af,4) + 8*Power(af,3)*jMax*tf - 24*Power(jMax,3)*tf*(p0 - pf + tf*v0) + 12*Power(af,2)*jMax*(v0 - vf) + 36*Power(jMax,2)*Power(v0 - vf,2) + 24*af*Power(jMax,2)*(p0 - pf + 2*tf*v0 - tf*vf)))/(144.*Power(jMax,5)*(a0 - af + jMax*tf)); std::array deriv; deriv[0] = 5 * polynom[0]; @@ -246,14 +246,14 @@ bool RuckigStep2::time_up_vel(Profile& profile, double vMax, double aMax, double { // Find root of 6th order polynom std::array polynom; - polynom[0] = 144*Power(jMax,6); - polynom[1] = -144*Power(jMax,5)*(-5*a0 + jMax*tf); - polynom[2] = 36*Power(jMax,4)*(39*Power(a0,2) - 16*a0*jMax*tf + 2*jMax*(jMax*Power(tf,2) + 3*v0 - 3*vf)); - polynom[3] = 24*Power(jMax,3)*(55*Power(a0,3) - 33*Power(a0,2)*jMax*tf - 6*Power(jMax,2)*(p0 - pf + tf*v0) + 12*a0*jMax*(jMax*Power(tf,2) + 3*v0 - 3*vf)); - polynom[4] = 6*Power(jMax,2)*(101*Power(a0,4) - 76*Power(a0,3)*jMax*tf - 48*a0*Power(jMax,2)*(p0 - pf + tf*v0) + 12*Power(jMax,2)*(2*jMax*tf*(p0 - pf + tf*v0) + 3*Power(v0 - vf,2)) + 60*Power(a0,2)*jMax*(jMax*Power(tf,2) + 3*v0 - 3*vf)); - polynom[5] = 12*a0*jMax*(11*Power(a0,4) - 10*Power(a0,3)*jMax*tf - 12*a0*Power(jMax,2)*(p0 - pf + tf*v0) + 12*Power(jMax,2)*(2*jMax*tf*(p0 - pf + tf*v0) + 3*Power(v0 - vf,2)) + 12*Power(a0,2)*jMax*(jMax*Power(tf,2) + 3*v0 - 3*vf)); - polynom[6] = 11*Power(a0,6) - 12*Power(a0,5)*jMax*tf - 24*Power(a0,3)*Power(jMax,2)*(p0 - pf + tf*v0) + 36*Power(a0,2)*Power(jMax,2)*(2*jMax*tf*(p0 - pf + tf*v0) + 3*Power(v0 - vf,2)) + 72*Power(jMax,3)*(jMax*Power(p0 - pf + tf*v0,2) + Power(v0 - vf,3)) + 18*Power(a0,4)*jMax*(jMax*Power(tf,2) + 3*v0 - 3*vf); - + polynom[0] = 1.0; + polynom[1] = -((-5*a0 + af + jMax*tf)/jMax); + polynom[2] = (39*Power(a0,2) - Power(af,2) + 4*af*jMax*tf - 16*a0*(af + jMax*tf) + 2*jMax*(jMax*Power(tf,2) + 3*v0 - 3*vf))/(4.*Power(jMax,2)); + polynom[3] = (55*Power(a0,3) - 33*Power(a0,2)*(af + jMax*tf) - 6*a0*(Power(af,2) - 4*af*jMax*tf - 2*jMax*(jMax*Power(tf,2) + 3*v0 - 3*vf)) + 2*(Power(af,3) - 3*Power(jMax,2)*(p0 - pf + tf*v0) + 3*af*jMax*(-v0 + vf)))/(6.*Power(jMax,3)); + polynom[4] = (101*Power(a0,4) + Power(af,4) - 8*Power(af,3)*jMax*tf - 76*Power(a0,3)*(af + jMax*tf) - 30*Power(a0,2)*(Power(af,2) - 4*af*jMax*tf - 2*jMax*(jMax*Power(tf,2) + 3*v0 - 3*vf)) + 12*Power(jMax,2)*(2*jMax*tf*(p0 - pf + tf*v0) + 3*Power(v0 - vf,2)) - 12*Power(af,2)*jMax*(v0 - vf) + 24*af*Power(jMax,2)*(p0 - pf + 2*tf*v0 - tf*vf) + 16*a0*(Power(af,3) - 3*Power(jMax,2)*(p0 - pf + tf*v0) + 3*af*jMax*(-v0 + vf)))/(24.*Power(jMax,4)); + polynom[5] = (a0*(11*Power(a0,4) + Power(af,4) - 8*Power(af,3)*jMax*tf - 10*Power(a0,3)*(af + jMax*tf) - 6*Power(a0,2)*(Power(af,2) - 4*af*jMax*tf - 2*jMax*(jMax*Power(tf,2) + 3*v0 - 3*vf)) + 12*Power(jMax,2)*(2*jMax*tf*(p0 - pf + tf*v0) + 3*Power(v0 - vf,2)) - 12*Power(af,2)*jMax*(v0 - vf) + 24*af*Power(jMax,2)*(p0 - pf + 2*tf*v0 - tf*vf) + 4*a0*(Power(af,3) - 3*Power(jMax,2)*(p0 - pf + tf*v0) + 3*af*jMax*(-v0 + vf))))/(12.*Power(jMax,5)); + polynom[6] = (11*Power(a0,6) - Power(af,6) - 12*Power(a0,5)*(af + jMax*tf) - 48*Power(af,3)*Power(jMax,2)*(p0 - pf + tf*v0) - 9*Power(a0,4)*(Power(af,2) - 4*af*jMax*tf - 2*jMax*(jMax*Power(tf,2) + 3*v0 - 3*vf)) + 72*Power(jMax,3)*(jMax*Power(p0 - pf + tf*v0,2) + Power(v0 - vf,3)) + 6*Power(af,4)*jMax*(v0 - vf) + 144*af*Power(jMax,3)*(p0 - pf + tf*v0)*(v0 - vf) - 36*Power(af,2)*Power(jMax,2)*Power(v0 - vf,2) + 8*Power(a0,3)*(Power(af,3) - 3*Power(jMax,2)*(p0 - pf + tf*v0) + 3*af*jMax*(-v0 + vf)) + 3*Power(a0,2)*(Power(af,4) - 8*Power(af,3)*jMax*tf + 12*Power(jMax,2)*(2*jMax*tf*(p0 - pf + tf*v0) + 3*Power(v0 - vf,2)) - 12*Power(af,2)*jMax*(v0 - vf) + 24*af*Power(jMax,2)*(p0 - pf + 2*tf*v0 - tf*vf)))/(144.*Power(jMax,6)); + std::array deriv; deriv[0] = 6 * polynom[0]; deriv[1] = 5 * polynom[1]; @@ -314,6 +314,7 @@ bool RuckigStep2::time_up_vel(Profile& profile, double vMax, double aMax, double double vPlat = Power(a0,2)/(2.*jMax) + 2*a0*tz + jMax*Power(tz,2) + v0; // std::cout << "BE CAREFUL " << vPlat << std::endl; + // TODO: for af profile.t[0] = tz; profile.t[1] = 0; profile.t[2] = a0/jMax + tz; @@ -560,11 +561,12 @@ bool RuckigStep2::time_up_none(Profile& profile, double vMax, double aMax, doubl // First constant, then acc { std::array polynom; - polynom[0] = 18*Power(jMax,4)*Power(a0 - jMax*tf,2); - polynom[1] = 36*Power(jMax,4)*(a0 - jMax*tf)*(jMax*Power(tf,2) + 2*v0 - 2*vf); - polynom[2] = 6*Power(jMax,2)*(-2*Power(a0,4) + 8*Power(a0,3)*jMax*tf - 12*Power(a0,2)*Power(jMax,2)*Power(tf,2) + 6*a0*Power(jMax,2)*(2*p0 - 2*pf + jMax*Power(tf,3) + 2*tf*vf) + 3*Power(jMax,2)*(Power(jMax,2)*Power(tf,4) + 4*Power(v0 - vf,2) + 4*jMax*tf*(-p0 + pf + tf*v0 - 2*tf*vf))); - polynom[3] = 6*jMax*(Power(a0,5) - 5*Power(a0,4)*jMax*tf + 4*Power(a0,3)*jMax*(2*jMax*Power(tf,2) - v0 + vf) - 12*Power(a0,2)*Power(jMax,2)*(2*p0 - 2*pf - tf*v0 + 3*tf*vf) - 12*Power(jMax,3)*(jMax*Power(tf,2)*(p0 - pf + tf*v0) + (v0 - vf)*(-2*p0 + 2*pf + tf*v0 - 3*tf*vf)) - 6*a0*Power(jMax,2)*(Power(jMax,2)*Power(tf,4) - 2*Power(v0 - vf,2) - 8*jMax*tf*(p0 - pf + tf*vf))); - polynom[4] = -Power(a0,6) + 6*Power(a0,5)*jMax*tf + 72*Power(jMax,3)*(jMax*Power(p0 - pf + tf*v0,2) + Power(v0 - vf,3)) - 6*Power(a0,4)*jMax*(2*jMax*Power(tf,2) - v0 + vf) + 24*Power(a0,3)*Power(jMax,2)*(2*p0 - 2*pf - tf*v0 + 3*tf*vf) + 72*a0*Power(jMax,3)*(jMax*Power(tf,2)*(p0 - pf + tf*v0) + (v0 - vf)*(-2*p0 + 2*pf + tf*v0 - 3*tf*vf)) + 18*Power(a0,2)*Power(jMax,2)*(Power(jMax,2)*Power(tf,4) - 2*Power(v0 - vf,2) - 8*jMax*tf*(p0 - pf + tf*vf)); + polynom[0] = 1.0; + polynom[1] = (4*af*tf - 2*jMax*Power(tf,2) + 4*v0 - 4*vf)/(a0 - af + jMax*tf); + polynom[2] = (-2*Power(a0,4) - 2*Power(af,4) + 8*Power(af,3)*jMax*tf + 6*Power(af,2)*Power(jMax,2)*Power(tf,2) + 8*Power(a0,3)*(af - jMax*tf) - 12*Power(a0,2)*Power(af - jMax*tf,2) - 12*af*Power(jMax,2)*(p0 - pf + jMax*Power(tf,3) - 2*tf*v0 + 3*tf*vf) + 2*a0*(4*Power(af,3) - 12*Power(af,2)*jMax*tf + 9*af*Power(jMax,2)*Power(tf,2) - 3*Power(jMax,2)*(-2*p0 + 2*pf + jMax*Power(tf,3) - 2*tf*vf)) + 3*Power(jMax,2)*(Power(jMax,2)*Power(tf,4) + 4*Power(v0 - vf,2) - 4*jMax*tf*(-p0 + pf + tf*v0 - 2*tf*vf)))/(3.*Power(jMax,2)*Power(a0 - af + jMax*tf,2)); + polynom[3] = (-Power(a0,5) + Power(af,5) - Power(af,4)*jMax*tf + 5*Power(a0,4)*(af - jMax*tf) - 2*Power(a0,3)*(5*Power(af,2) - 8*af*jMax*tf + 2*jMax*(2*jMax*Power(tf,2) + v0 - vf)) - 4*Power(af,3)*jMax*(jMax*Power(tf,2) - v0 + vf) + 12*Power(af,2)*Power(jMax,2)*(2*p0 - 2*pf + tf*(v0 + vf)) - 12*af*Power(jMax,2)*(-Power(v0 - vf,2) + jMax*tf*(2*p0 - 2*pf + 3*tf*v0 - tf*vf)) + 2*Power(a0,2)*(5*Power(af,3) - 9*Power(af,2)*jMax*tf + 6*af*jMax*(v0 - vf) + 6*Power(jMax,2)*(2*p0 - 2*pf - tf*v0 + 3*tf*vf)) + 12*Power(jMax,3)*(jMax*Power(tf,2)*(p0 - pf + tf*v0) + (v0 - vf)*(2*p0 - 2*pf - tf*v0 + 3*tf*vf)) + a0*(-5*Power(af,4) + 8*Power(af,3)*jMax*tf + 12*Power(af,2)*jMax*(jMax*Power(tf,2) - v0 + vf) - 24*af*Power(jMax,2)*(2*p0 - 2*pf + jMax*Power(tf,3) + 2*tf*vf) + 6*Power(jMax,2)*(Power(jMax,2)*Power(tf,4) - 2*Power(v0 - vf,2) + 8*jMax*tf*(p0 - pf + tf*vf))))/(3.*Power(jMax,3)*Power(a0 - af + jMax*tf,2)); + polynom[4] = -(Power(a0,6) + Power(af,6) - 6*Power(a0,5)*(af - jMax*tf) + 48*Power(af,3)*Power(jMax,2)*(p0 - pf + tf*v0) - 72*Power(jMax,3)*(jMax*Power(p0 - pf + tf*v0,2) - Power(v0 - vf,3)) + 3*Power(a0,4)*(5*Power(af,2) - 8*af*jMax*tf + 2*jMax*(2*jMax*Power(tf,2) + v0 - vf)) + 6*Power(af,4)*jMax*(v0 - vf) + 144*af*Power(jMax,3)*(p0 - pf + tf*v0)*(v0 - vf) + 36*Power(af,2)*Power(jMax,2)*Power(v0 - vf,2) - 4*Power(a0,3)*(5*Power(af,3) - 9*Power(af,2)*jMax*tf + 6*af*jMax*(v0 - vf) + 6*Power(jMax,2)*(2*p0 - 2*pf - tf*v0 + 3*tf*vf)) + 3*Power(a0,2)*(5*Power(af,4) - 8*Power(af,3)*jMax*tf - 12*Power(af,2)*jMax*(jMax*Power(tf,2) - v0 + vf) + 24*af*Power(jMax,2)*(2*p0 - 2*pf + jMax*Power(tf,3) + 2*tf*vf) - 6*Power(jMax,2)*(Power(jMax,2)*Power(tf,4) - 2*Power(v0 - vf,2) + 8*jMax*tf*(p0 - pf + tf*vf))) - 6*a0*(Power(af,5) - Power(af,4)*jMax*tf - 4*Power(af,3)*jMax*(jMax*Power(tf,2) - v0 + vf) + 12*Power(af,2)*Power(jMax,2)*(2*p0 - 2*pf + tf*(v0 + vf)) - 12*af*Power(jMax,2)*(-Power(v0 - vf,2) + jMax*tf*(2*p0 - 2*pf + 3*tf*v0 - tf*vf)) + 12*Power(jMax,3)*(jMax*Power(tf,2)*(p0 - pf + tf*v0) + (v0 - vf)*(2*p0 - 2*pf - tf*v0 + 3*tf*vf))))/(18.*Power(jMax,4)*Power(a0 - af + jMax*tf,2)); + auto roots = Roots::solveQuart(polynom); for (double t: roots) { @@ -572,22 +574,14 @@ bool RuckigStep2::time_up_none(Profile& profile, double vMax, double aMax, doubl continue; } - profile.t[0] = 0; - profile.t[1] = 0; - profile.t[2] = t; - profile.t[3] = -((Power(a0,5) - Power(a0,4)*jMax*(4*t + 5*tf) + 2*Power(a0,3)*jMax*(jMax*(3*Power(t,2) + 8*t*tf + 4*Power(tf,2)) - 2*v0 + 2*vf) + 6*Power(a0,2)*Power(jMax,2)*(-10*p0 + 10*pf + 2*jMax*Power(t,3) - 3*jMax*Power(t,2)*tf - 3*jMax*t*Power(tf,2) + jMax*Power(tf,3) + 2*t*v0 + 2*tf*v0 - 2*t*vf - 12*tf*vf) + 12*Power(jMax,3)*(Power(jMax,2)*t*Power(t - tf,2)*Power(tf,2) + (v0 - vf)*(2*p0 - 2*pf + 2*t*v0 - 3*tf*v0 - 2*t*vf + 5*tf*vf) - jMax*tf*(2*p0*(t + 2*tf) - 2*pf*(t + 2*tf) + 3*Power(t,2)*v0 - 3*t*tf*v0 + 3*Power(tf,2)*v0 - 3*Power(t,2)*vf + 5*t*tf*vf + Power(tf,2)*vf)) - 12*a0*Power(jMax,2)*(Power(jMax,2)*Power(t - tf,2)*tf*(2*t + tf) - 3*Power(v0 - vf,2) - jMax*(2*p0*(t + 5*tf) - 2*pf*(t + 5*tf) + 3*Power(t,2)*v0 - 2*t*tf*v0 + 2*Power(tf,2)*v0 - 3*Power(t,2)*vf + 4*t*tf*vf + 8*Power(tf,2)*vf)))/(jMax*(Power(a0,4) - 4*Power(a0,3)*jMax*tf + 6*Power(a0,2)*Power(jMax,2)*Power(tf,2) - 24*a0*Power(jMax,2)*(p0 - pf + tf*vf) + 12*Power(jMax,2)*(Power(v0 - vf,2) + jMax*tf*(2*p0 - 2*pf + tf*(v0 + vf)))))); - profile.t[4] = (-2*Power(a0,4)*t + Power(a0,3)*(jMax*(3*Power(t,2) + 8*t*tf - Power(tf,2)) - 2*v0 + 2*vf) + 3*Power(a0,2)*jMax*(-6*p0 + 6*pf + 2*jMax*Power(t,3) - 3*jMax*Power(t,2)*tf - 3*jMax*t*Power(tf,2) + 2*jMax*Power(tf,3) + 2*t*v0 + 2*tf*v0 - 2*t*vf - 8*tf*vf) + 6*Power(jMax,2)*(Power(jMax,2)*t*Power(t - tf,2)*Power(tf,2) + 2*(v0 - vf)*(p0 - pf + t*v0 - tf*v0 - t*vf + 2*tf*vf) + jMax*tf*(-2*p0*(t + tf) + 2*pf*(t + tf) - 3*Power(t,2)*v0 + 3*t*tf*v0 - 2*Power(tf,2)*v0 + 3*Power(t,2)*vf - 5*t*tf*vf)) - 6*a0*jMax*(Power(jMax,2)*Power(t - tf,2)*tf*(2*t + tf) - 2*Power(v0 - vf,2) - jMax*(2*p0*(t + 3*tf) - 2*pf*(t + 3*tf) + 3*Power(t,2)*v0 - 2*t*tf*v0 + Power(tf,2)*v0 - 3*Power(t,2)*vf + 4*t*tf*vf + 5*Power(tf,2)*vf)))/(Power(a0,4) - 4*Power(a0,3)*jMax*tf + 6*Power(a0,2)*Power(jMax,2)*Power(tf,2) - 24*a0*Power(jMax,2)*(p0 - pf + tf*vf) + 12*Power(jMax,2)*(Power(v0 - vf,2) + jMax*tf*(2*p0 - 2*pf + tf*(v0 + vf)))); + profile.t[0] = t; + profile.t[1] = -((-Power(a0,5) + Power(af,5) - Power(af,4)*jMax*(4*t + tf) + Power(a0,4)*(5*af - 4*jMax*t - 5*jMax*tf) - 2*Power(a0,3)*(5*Power(af,2) - 8*af*jMax*(t + tf) + jMax*(jMax*(t + 2*tf)*(3*t + 2*tf) + 2*(v0 - vf))) + 2*Power(af,3)*jMax*(jMax*(3*Power(t,2) + 2*t*tf + Power(tf,2)) + 2*(v0 - vf)) + 6*Power(af,2)*Power(jMax,2)*(10*p0 - 10*pf + jMax*(2*Power(t,3) - 9*Power(t,2)*tf + 7*t*Power(tf,2) - Power(tf,3)) - 2*t*v0 + 10*tf*v0 + 2*t*vf) + 12*Power(jMax,3)*(Power(jMax,2)*t*Power(t - tf,2)*Power(tf,2) + (v0 - vf)*(2*p0 - 2*pf + 2*t*v0 - 3*tf*v0 - 2*t*vf + 5*tf*vf) + jMax*tf*(2*p0*(t + 2*tf) - 2*pf*(t + 2*tf) + 3*Power(t,2)*v0 - 3*t*tf*v0 + 3*Power(tf,2)*v0 - 3*Power(t,2)*vf + 5*t*tf*vf + Power(tf,2)*vf)) + 2*Power(a0,2)*(5*Power(af,3) - 3*Power(af,2)*jMax*(4*t + 3*tf) + 3*af*jMax*(3*jMax*(Power(t,2) + 2*t*tf - Power(tf,2)) + 2*(v0 - vf)) + 3*Power(jMax,2)*(10*p0 - 10*pf + jMax*(2*Power(t,3) - 3*Power(t,2)*tf - 3*t*Power(tf,2) + Power(tf,3)) - 2*(t + tf)*v0 + 2*(t + 6*tf)*vf)) - 12*af*Power(jMax,2)*(2*Power(jMax,2)*t*(t - 2*tf)*(t - tf)*tf - 3*Power(v0 - vf,2) + jMax*(2*p0*(t + 4*tf) - 2*pf*(t + 4*tf) + 3*(Power(t,2) - 2*t*tf + 3*Power(tf,2))*v0 - (3*Power(t,2) - 8*t*tf + Power(tf,2))*vf)) + a0*(-5*Power(af,4) + 8*Power(af,3)*jMax*(2*t + tf) - 6*Power(af,2)*jMax*(jMax*(3*t - 2*tf)*(t + 2*tf) + 2*(v0 - vf)) - 24*af*Power(jMax,2)*(5*p0 - 5*pf + jMax*(Power(t,3) - 3*Power(t,2)*tf + 2*Power(tf,3)) - t*v0 + 2*tf*v0 + t*vf + 3*tf*vf) + 12*Power(jMax,2)*(Power(jMax,2)*Power(t - tf,2)*tf*(2*t + tf) - 3*Power(v0 - vf,2) + jMax*(2*p0*(t + 5*tf) - 2*pf*(t + 5*tf) + 3*Power(t,2)*v0 - 2*t*tf*v0 + 2*Power(tf,2)*v0 - 3*Power(t,2)*vf + 4*t*tf*vf + 8*Power(tf,2)*vf))))/(jMax*(Power(a0,4) + Power(af,4) - 4*Power(af,3)*jMax*tf + 6*Power(af,2)*Power(jMax,2)*Power(tf,2) - 4*Power(a0,3)*(af - jMax*tf) + 6*Power(a0,2)*Power(af - jMax*tf,2) + 24*af*Power(jMax,2)*(p0 - pf + tf*v0) - 4*a0*(Power(af,3) - 3*Power(af,2)*jMax*tf + 6*Power(jMax,2)*(p0 - pf + tf*vf)) + 12*Power(jMax,2)*(Power(v0 - vf,2) - jMax*tf*(2*p0 - 2*pf + tf*(v0 + vf)))))); + profile.t[2] = (-2*Power(a0,4)*t + 2*Power(af,4)*(-t + tf) + Power(af,3)*(3*jMax*Power(t,2) + 2*jMax*t*tf - 4*jMax*Power(tf,2) + 2*v0 - 2*vf) + Power(a0,3)*(t*(8*af - 3*jMax*t) - 2*(af + 4*jMax*t)*tf + jMax*Power(tf,2) - 2*v0 + 2*vf) + 3*Power(af,2)*jMax*(6*p0 - 6*pf + jMax*t*(2*t - 7*tf)*(t - tf) - 2*t*v0 + 6*tf*v0 + 2*t*vf) + 6*Power(jMax,2)*(Power(jMax,2)*t*Power(t - tf,2)*Power(tf,2) + 2*(v0 - vf)*(p0 - pf + t*v0 - tf*v0 - t*vf + 2*tf*vf) + jMax*tf*(2*p0*(t + tf) - 2*pf*(t + tf) + 3*Power(t,2)*v0 - 3*t*tf*v0 + 2*Power(tf,2)*v0 - 3*Power(t,2)*vf + 5*t*tf*vf)) - 6*af*jMax*(2*Power(jMax,2)*t*(t - 2*tf)*(t - tf)*tf - 2*Power(v0 - vf,2) + jMax*(2*p0*(t + 2*tf) - 2*pf*(t + 2*tf) + 3*Power(t,2)*v0 - 6*t*tf*v0 + 6*Power(tf,2)*v0 - 3*Power(t,2)*vf + 8*t*tf*vf - 2*Power(tf,2)*vf)) + 3*Power(a0,2)*(Power(jMax,2)*(t - 2*tf)*(2*t - tf)*(t + tf) + 2*af*(af*(-2*t + tf) + v0 - vf) + jMax*(6*p0 - 6*pf + 3*af*(Power(t,2) + 2*t*tf - 2*Power(tf,2)) - 2*(t + tf)*v0 + 2*(t + 4*tf)*vf)) + a0*(Power(af,3)*(8*t - 6*tf) - 3*Power(af,2)*(jMax*(t - tf)*(3*t + 7*tf) + 2*(v0 - vf)) - 12*af*jMax*(3*p0 - 3*pf + jMax*(Power(t,3) - 3*Power(t,2)*tf + 2*Power(tf,3)) - t*v0 + tf*v0 + t*vf + 2*tf*vf) + 6*jMax*(Power(jMax,2)*Power(t - tf,2)*tf*(2*t + tf) - 2*Power(v0 - vf,2) + jMax*(2*p0*(t + 3*tf) - 2*pf*(t + 3*tf) + 3*Power(t,2)*v0 - 2*t*tf*v0 + Power(tf,2)*v0 - 3*Power(t,2)*vf + 4*t*tf*vf + 5*Power(tf,2)*vf))))/(Power(a0,4) + Power(af,4) - 4*Power(af,3)*jMax*tf + 6*Power(af,2)*Power(jMax,2)*Power(tf,2) - 4*Power(a0,3)*(af - jMax*tf) + 6*Power(a0,2)*Power(af - jMax*tf,2) + 24*af*Power(jMax,2)*(p0 - pf + tf*v0) - 4*a0*(Power(af,3) - 3*Power(af,2)*jMax*tf + 6*Power(jMax,2)*(p0 - pf + tf*vf)) + 12*Power(jMax,2)*(Power(v0 - vf,2) - jMax*tf*(2*p0 - 2*pf + tf*(v0 + vf)))); + profile.t[3] = 0; + profile.t[4] = -((Power(a0,5) - Power(af,5) + Power(af,4)*jMax*(3*t + 2*tf) + Power(a0,4)*(-5*af + 3*jMax*t + 4*jMax*tf) + Power(a0,3)*(10*Power(af,2) - 2*af*jMax*(6*t + 7*tf) + jMax*(3*jMax*Power(t,2) + 12*jMax*t*tf + 5*jMax*Power(tf,2) + 2*v0 - 2*vf)) - Power(af,3)*jMax*(3*jMax*Power(t,2) + 6*jMax*t*tf + 2*jMax*Power(tf,2) + 2*v0 - 2*vf) - 3*Power(af,2)*Power(jMax,2)*(14*p0 - 14*pf + jMax*t*(2*Power(t,2) - 9*t*tf + 5*Power(tf,2)) - 2*t*v0 + 14*tf*v0 + 2*t*vf) + 6*Power(jMax,3)*(-(Power(jMax,2)*t*Power(t - tf,2)*Power(tf,2)) + jMax*tf*(6*pf*t + 2*pf*tf - 2*p0*(3*t + tf) - 3*Power(t,2)*v0 + t*tf*v0 - 2*Power(tf,2)*v0 + t*(3*t - 7*tf)*vf) + 2*(v0 - vf)*(-p0 + pf + tf*v0 - 2*tf*vf)) + 6*af*Power(jMax,2)*(2*Power(jMax,2)*t*(t - 2*tf)*(t - tf)*tf - 4*Power(v0 - vf,2) + jMax*(6*p0*t - 6*pf*t + 8*p0*tf - 8*pf*tf + 3*Power(t,2)*v0 - 2*t*tf*v0 + 8*Power(tf,2)*v0 - 3*Power(t,2)*vf + 8*t*tf*vf)) + Power(a0,2)*(-10*Power(af,3) + 18*Power(af,2)*jMax*(t + tf) - 3*af*jMax*(3*jMax*Power(t,2) + 10*jMax*t*tf - 4*jMax*Power(tf,2) + 2*v0 - 2*vf) + 3*Power(jMax,2)*(-14*p0 + 14*pf + jMax*(-2*Power(t,3) + 3*Power(t,2)*tf + 5*t*Power(tf,2) - 2*Power(tf,3)) + 2*(t + tf)*v0 - 2*(t + 8*tf)*vf)) + a0*(5*Power(af,4) - 2*Power(af,3)*jMax*(6*t + 5*tf) + 3*Power(af,2)*jMax*(3*jMax*Power(t,2) + 8*jMax*t*tf - 5*jMax*Power(tf,2) + 2*v0 - 2*vf) + 12*af*Power(jMax,2)*(7*p0 - 7*pf + jMax*(Power(t,3) - 3*Power(t,2)*tf + 2*Power(tf,3)) - t*v0 + 3*tf*v0 + t*vf + 4*tf*vf) - 6*Power(jMax,2)*(Power(jMax,2)*Power(t - tf,2)*tf*(2*t + tf) - 4*Power(v0 - vf,2) + jMax*(6*p0*t - 6*pf*t + 10*p0*tf - 10*pf*tf + 3*Power(t,2)*v0 - 2*t*tf*v0 + 3*Power(tf,2)*v0 - 3*Power(t,2)*vf + 8*t*tf*vf + 7*Power(tf,2)*vf))))/(jMax*(Power(a0,4) + Power(af,4) - 4*Power(af,3)*jMax*tf + 6*Power(af,2)*Power(jMax,2)*Power(tf,2) - 4*Power(a0,3)*(af - jMax*tf) + 6*Power(a0,2)*Power(af - jMax*tf,2) + 24*af*Power(jMax,2)*(p0 - pf + tf*v0) - 4*a0*(Power(af,3) - 3*Power(af,2)*jMax*tf + 6*Power(jMax,2)*(p0 - pf + tf*vf)) + 12*Power(jMax,2)*(Power(v0 - vf,2) - jMax*tf*(2*p0 - 2*pf + tf*(v0 + vf)))))); profile.t[5] = 0; - profile.t[6] = (Power(a0,5) - Power(a0,4)*jMax*(3*t + 4*tf) + Power(a0,3)*jMax*(jMax*(3*Power(t,2) + 12*t*tf + 5*Power(tf,2)) - 2*v0 + 2*vf) + 3*Power(a0,2)*Power(jMax,2)*(-14*p0 + 14*pf + 2*jMax*Power(t,3) - 3*jMax*Power(t,2)*tf - 5*jMax*t*Power(tf,2) + 2*jMax*Power(tf,3) + 2*t*v0 + 2*tf*v0 - 2*t*vf - 16*tf*vf) + 6*Power(jMax,3)*(Power(jMax,2)*t*Power(t - tf,2)*Power(tf,2) - 2*(v0 - vf)*(-p0 + pf + tf*v0 - 2*tf*vf) + jMax*tf*(6*pf*t + 2*pf*tf - 2*p0*(3*t + tf) - 3*Power(t,2)*v0 + t*tf*v0 - 2*Power(tf,2)*v0 + 3*Power(t,2)*vf - 7*t*tf*vf)) - 6*a0*Power(jMax,2)*(Power(jMax,2)*Power(t - tf,2)*tf*(2*t + tf) - 4*Power(v0 - vf,2) - jMax*(6*p0*t - 6*pf*t + 10*p0*tf - 10*pf*tf + 3*Power(t,2)*v0 - 2*t*tf*v0 + 3*Power(tf,2)*v0 - 3*Power(t,2)*vf + 8*t*tf*vf + 7*Power(tf,2)*vf)))/(jMax*(Power(a0,4) - 4*Power(a0,3)*jMax*tf + 6*Power(a0,2)*Power(jMax,2)*Power(tf,2) - 24*a0*Power(jMax,2)*(p0 - pf + tf*vf) + 12*Power(jMax,2)*(Power(v0 - vf,2) + jMax*tf*(2*p0 - 2*pf + tf*(v0 + vf))))); - - // std::cout << t << 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 << "---" << std::endl; - + profile.t[6] = 0; + profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, jMax, 0, -jMax}); if (profile.check(tf, pf, vf, af, vMax, aMax)) { return true; @@ -670,9 +664,6 @@ bool RuckigStep2::get_profile(Profile& profile, double vMax, double aMax, double } else if (time_up_acc0_acc1(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC0_ACC1; - } else if (time_down_none(profile, vMax, aMax, jMax)) { - profile.type = Profile::Type::DOWN_NONE; - } else if (time_down_acc0(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC0; @@ -682,6 +673,9 @@ bool RuckigStep2::get_profile(Profile& profile, double vMax, double aMax, double } else if (time_down_acc0_acc1(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC0_ACC1; + } else if (time_down_none(profile, vMax, aMax, jMax)) { + profile.type = Profile::Type::DOWN_NONE; + } else { return false; } @@ -723,9 +717,6 @@ bool RuckigStep2::get_profile(Profile& profile, double vMax, double aMax, double } else if (time_down_acc0_acc1(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC0_ACC1; - } else if (time_up_none(profile, vMax, aMax, jMax)) { - profile.type = Profile::Type::UP_NONE; - } else if (time_up_acc0(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC0; @@ -735,6 +726,9 @@ bool RuckigStep2::get_profile(Profile& profile, double vMax, double aMax, double } else if (time_up_acc0_acc1(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC0_ACC1; + } else if (time_up_none(profile, vMax, aMax, jMax)) { + profile.type = Profile::Type::UP_NONE; + } else { return false; } diff --git a/test/otg-test.cpp b/test/otg-test.cpp index e5a1fbd0..76780cea 100644 --- a/test/otg-test.cpp +++ b/test/otg-test.cpp @@ -198,6 +198,34 @@ TEST_CASE("Ruckig") { } } + SECTION("Random input with 1 DoF with target velocity, acceleration") { + Ruckig<1> otg {0.005}; + InputParameter<1> input; + + srand(47); + std::default_random_engine gen; + std::uniform_real_distribution dist(0.0, 1.0); + + for (size_t i = 0; i < 12*1024; i += 1) { + input.current_position = Vec1::Random(); + input.current_velocity = dist(gen) < 0.9 ? (Vec1)Vec1::Random() : (Vec1)Vec1::Zero(); + input.current_acceleration = dist(gen) < 0.8 ? (Vec1)Vec1::Random() : (Vec1)Vec1::Zero(); + input.target_position = Vec1::Random(); + input.target_velocity = dist(gen) < 0.6 ? (Vec1)Vec1::Random() : (Vec1)Vec1::Zero(); + input.target_acceleration = dist(gen) < 0.4 ? (Vec1)Vec1::Random() : (Vec1)Vec1::Zero(); + input.max_velocity = 10 * Vec1::Random().array().abs() + input.target_velocity.array().abs(); // Target velocity needs to be smaller than max velocity + input.max_acceleration = 10 * Vec1::Random().array().abs() + input.target_acceleration.array().abs() + 0.1; + input.max_jerk = 10 * Vec1::Random().array().abs() + 0.1; + + auto max_target_acceleration = (2 * input.max_jerk.array() * (input.max_velocity.array() - input.target_velocity.array().abs())).sqrt(); + if ((input.target_acceleration.array().abs() > max_target_acceleration.array()).any()) { + continue; + } + + check_calculation(otg, input); + } + } + #ifdef WITH_REFLEXXES SECTION("Comparison with Reflexxes with 1 DoF") { Ruckig<1> otg {0.005}; @@ -231,7 +259,7 @@ TEST_CASE("Ruckig") { std::default_random_engine gen; std::uniform_real_distribution dist(0.0, 1.0); - for (size_t i = 0; i < 96; i += 1) { + for (size_t i = 0; i < 1024; i += 1) { input.current_position = Vec2::Random(); input.current_velocity = dist(gen) < 0.9 ? (Vec2)Vec2::Random() : (Vec2)Vec2::Zero(); input.current_acceleration = dist(gen) < 0.8 ? (Vec2)Vec2::Random() : (Vec2)Vec2::Zero(); diff --git a/test/otg_plot.py b/test/otg_plot.py index 070200ea..fc21fa44 100644 --- a/test/otg_plot.py +++ b/test/otg_plot.py @@ -95,18 +95,18 @@ def print_input_for_mathematica(inp, dof, tf=None): if __name__ == '__main__': inp = InputParameter() - inp.current_position = [-0.2684100202] - inp.current_velocity = [0.8327910499] - inp.current_acceleration = [0.7191754485] - inp.target_position = [-0.8182368855] - inp.target_velocity = [0] - inp.target_acceleration = [0] - inp.max_velocity = [1.073347279] - inp.max_acceleration = [0.3522756207] - inp.max_jerk = [0.103642817] + inp.current_position = [-0.9579634331] + inp.current_velocity = [-0.491420063] + inp.current_acceleration = [0.7030019433] + inp.target_position = [-0.6463393167] + inp.target_velocity = [0.9751044297] + inp.target_acceleration = [0.5401507661] + inp.max_velocity = [6.914363305] + inp.max_acceleration = [1.804067883] + inp.max_jerk = [9.774990577] inp.minimum_duration = None - # print_input_for_mathematica(inp, 0) + print_input_for_mathematica(inp, 0) # otg = Quintic(0.005) # otg = Smoothie(0.005)