From 389fe249ea6b69be4afde706c2cf312fc2c257c3 Mon Sep 17 00:00:00 2001 From: pantor Date: Mon, 21 Dec 2020 13:20:11 +0100 Subject: [PATCH] improve tests --- include/ruckig/ruckig.hpp | 98 ++--- include/ruckig/wolfram.hpp | 4 - notebooks/brake.nb | 358 ++++++--------- notebooks/ruckig-step2.nb | 870 +++++++++++++++++++++++++++++++------ src/brake.cpp | 7 +- src/profile.cpp | 7 +- src/step1.cpp | 118 ++--- src/step2.cpp | 267 ++++++------ test/otg-test.cpp | 38 +- test/otg_plot.py | 16 +- 10 files changed, 1155 insertions(+), 628 deletions(-) diff --git a/include/ruckig/ruckig.hpp b/include/ruckig/ruckig.hpp index 1c02da67..e7a51d74 100644 --- a/include/ruckig/ruckig.hpp +++ b/include/ruckig/ruckig.hpp @@ -29,6 +29,7 @@ struct Profile { void set(double p0, double v0, double a0, const std::array& j); bool check(double pf, double vf, double af, double vMax, double aMax) const; + bool check(double tf, double pf, double vf, double af, double vMax, double aMax) const; //! Integrate with constant jerk for duration t. Returns new position, new velocity, and new acceleration. static std::tuple integrate(double t, double p0, double v0, double a0, double j); @@ -36,63 +37,61 @@ struct Profile { struct RuckigStep1 { - // double p0, v0, a0; - // double pf, vf, af; - // double vMax, aMax, jMax; + double p0, v0, a0; + double pf, vf, af; explicit RuckigStep1(double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_up_acc0_acc1_vel(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_up_acc1_vel(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_up_acc0_vel(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_up_vel(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_up_acc0_acc1(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_up_acc1(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_up_acc0(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_up_none(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - - bool time_down_acc0_acc1_vel(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_down_acc1_vel(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_down_acc0_vel(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_down_vel(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_down_acc0_acc1(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_down_acc1(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_down_acc0(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_down_none(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - - bool get_profile(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); + bool time_up_acc0_acc1_vel(Profile& profile, double vMax, double aMax, double jMax); + bool time_up_acc1_vel(Profile& profile, double vMax, double aMax, double jMax); + bool time_up_acc0_vel(Profile& profile, double vMax, double aMax, double jMax); + bool time_up_vel(Profile& profile, double vMax, double aMax, double jMax); + bool time_up_acc0_acc1(Profile& profile, double vMax, double aMax, double jMax); + bool time_up_acc1(Profile& profile, double vMax, double aMax, double jMax); + bool time_up_acc0(Profile& profile, double vMax, double aMax, double jMax); + bool time_up_none(Profile& profile, double vMax, double aMax, double jMax); + + bool time_down_acc0_acc1_vel(Profile& profile, double vMax, double aMax, double jMax); + bool time_down_acc1_vel(Profile& profile, double vMax, double aMax, double jMax); + bool time_down_acc0_vel(Profile& profile, double vMax, double aMax, double jMax); + bool time_down_vel(Profile& profile, double vMax, double aMax, double jMax); + bool time_down_acc0_acc1(Profile& profile, double vMax, double aMax, double jMax); + bool time_down_acc1(Profile& profile, double vMax, double aMax, double jMax); + bool time_down_acc0(Profile& profile, double vMax, double aMax, double jMax); + bool time_down_none(Profile& profile, double vMax, double aMax, double jMax); + + bool get_profile(Profile& profile, double vMax, double aMax, double jMax); static void get_brake_trajectory(double v0, double a0, double vMax, double aMax, double jMax, std::array& t_brake, std::array& j_brake); }; struct RuckigStep2 { - // double tf; - // double p0, v0, a0; - // double pf, vf, af; - // double vMax, aMax, jMax; + double tf; + double p0, v0, a0; + double pf, vf, af; explicit RuckigStep2(double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_up_acc0_acc1_vel(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_up_acc1_vel(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_up_acc0_vel(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_up_vel(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_up_acc0_acc1(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_up_acc1(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_up_acc0(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_up_none(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - - bool time_down_acc0_acc1_vel(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_down_acc1_vel(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_down_acc0_vel(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_down_vel(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_down_acc0_acc1(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_down_acc1(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_down_acc0(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - bool time_down_none(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); - - bool get_profile(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax); + bool time_up_acc0_acc1_vel(Profile& profile, double vMax, double aMax, double jMax); + bool time_up_acc1_vel(Profile& profile, double vMax, double aMax, double jMax); + bool time_up_acc0_vel(Profile& profile, double vMax, double aMax, double jMax); + bool time_up_vel(Profile& profile, double vMax, double aMax, double jMax); + bool time_up_acc0_acc1(Profile& profile, double vMax, double aMax, double jMax); + bool time_up_acc1(Profile& profile, double vMax, double aMax, double jMax); + bool time_up_acc0(Profile& profile, double vMax, double aMax, double jMax); + bool time_up_none(Profile& profile, double vMax, double aMax, double jMax); + + bool time_down_acc0_acc1_vel(Profile& profile, double vMax, double aMax, double jMax); + bool time_down_acc1_vel(Profile& profile, double vMax, double aMax, double jMax); + bool time_down_acc0_vel(Profile& profile, double vMax, double aMax, double jMax); + bool time_down_vel(Profile& profile, double vMax, double aMax, double jMax); + bool time_down_acc0_acc1(Profile& profile, double vMax, double aMax, double jMax); + bool time_down_acc1(Profile& profile, double vMax, double aMax, double jMax); + bool time_down_acc0(Profile& profile, double vMax, double aMax, double jMax); + bool time_down_none(Profile& profile, double vMax, double aMax, double jMax); + + bool get_profile(Profile& profile, double vMax, double aMax, double jMax); }; @@ -111,11 +110,6 @@ class Ruckig { return false; } - if (DOFs > 1 && (input.target_velocity.array() != 0.0).any()) { - std::cerr << "Ruckig does not support a target velocity for multiple DoFs." << std::endl; - return false; - } - if ((input.target_velocity.array().abs() > input.max_velocity.array()).any()) { std::cerr << "Target velocity exceeds maximal velocity." << std::endl; return false; @@ -168,7 +162,7 @@ class Ruckig { } RuckigStep1 step1 {p0s[dof], v0s[dof], a0s[dof], input.target_position[dof], input.target_velocity[dof], input.target_acceleration[dof], input.max_velocity[dof], input.max_acceleration[dof], input.max_jerk[dof]}; - bool found_profile = step1.get_profile(profiles[dof], p0s[dof], v0s[dof], a0s[dof], input.target_position[dof], input.target_velocity[dof], input.target_acceleration[dof], input.max_velocity[dof], input.max_acceleration[dof], input.max_jerk[dof]); + bool found_profile = step1.get_profile(profiles[dof], input.max_velocity[dof], input.max_acceleration[dof], input.max_jerk[dof]); if (!found_profile) { throw std::runtime_error("[ruckig] error in step 1: " + input.to_string(dof) + " all: " + input.to_string()); } @@ -188,7 +182,7 @@ class Ruckig { double t_profile = tf - profiles[dof].t_brake.value_or(0.0); RuckigStep2 step2 {t_profile, p0s[dof], v0s[dof], a0s[dof], input.target_position[dof], input.target_velocity[dof], input.target_acceleration[dof], input.max_velocity[dof], input.max_acceleration[dof], input.max_jerk[dof]}; - bool found_time_synchronization = step2.get_profile(profiles[dof], t_profile, p0s[dof], v0s[dof], a0s[dof], input.target_position[dof], input.target_velocity[dof], input.target_acceleration[dof], input.max_velocity[dof], input.max_acceleration[dof], input.max_jerk[dof]); + bool found_time_synchronization = step2.get_profile(profiles[dof], input.max_velocity[dof], input.max_acceleration[dof], input.max_jerk[dof]); if (!found_time_synchronization) { throw std::runtime_error("[ruckig] error in step 2: " + input.to_string(dof) + " all: " + input.to_string()); } diff --git a/include/ruckig/wolfram.hpp b/include/ruckig/wolfram.hpp index c4204d22..f2326230 100644 --- a/include/ruckig/wolfram.hpp +++ b/include/ruckig/wolfram.hpp @@ -7,10 +7,6 @@ inline double Power(double v, int e) { return std::pow(v, e); } -inline double Power(double v, double e) { - return std::pow(v, e); -} - inline double Sqrt(double v) { return std::sqrt(v); } diff --git a/notebooks/brake.nb b/notebooks/brake.nb index 16a17cd9..10ed3de4 100644 --- a/notebooks/brake.nb +++ b/notebooks/brake.nb @@ -10,10 +10,10 @@ NotebookFileLineBreakTest NotebookFileLineBreakTest NotebookDataPosition[ 158, 7] -NotebookDataLength[ 21971, 665] -NotebookOptionsPosition[ 19817, 622] -NotebookOutlinePosition[ 20209, 638] -CellTagsIndexPosition[ 20166, 635] +NotebookDataLength[ 19483, 573] +NotebookOptionsPosition[ 17110, 526] +NotebookOutlinePosition[ 17504, 542] +CellTagsIndexPosition[ 17461, 539] WindowFrame->Normal*) (* Beginning of Notebook Content *) @@ -177,211 +177,6 @@ Cell[BoxData[ CellLabel-> "In[778]:=",ExpressionUUID->"ccac5833-2cc2-4703-8af2-4673d2c0a437"], -Cell[CellGroupData[{ - -Cell[BoxData["resultT2"], "Input", - CellChangeTimes->{{3.814768806959013*^9, 3.814768831870268*^9}, { - 3.814768864975182*^9, 3.814768898647298*^9}, {3.815161660352097*^9, - 3.815161661764017*^9}, 3.817471849269354*^9}, - CellLabel-> - "In[779]:=",ExpressionUUID->"61a3a1a2-df88-4692-9d53-69048ae8b464"], - -Cell[BoxData[ - RowBox[{"{", - RowBox[{ - RowBox[{"{", - RowBox[{ - RowBox[{"t1", "\[Rule]", - RowBox[{"-", - FractionBox[ - RowBox[{ - SuperscriptBox[ - RowBox[{"(", - RowBox[{"-", - FractionBox["1", "2"]}], ")"}], - RowBox[{"1", "/", "3"}]], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - RowBox[{"-", "p0"}], "+", "pf"}], ")"}], - RowBox[{"1", "/", "3"}]]}], - SuperscriptBox["jMax", - RowBox[{"1", "/", "3"}]]]}]}], ",", - RowBox[{"t3", "\[Rule]", - RowBox[{"-", - FractionBox[ - RowBox[{ - SuperscriptBox[ - RowBox[{"(", - RowBox[{"-", - FractionBox["1", "2"]}], ")"}], - RowBox[{"1", "/", "3"}]], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - RowBox[{"-", "p0"}], "+", "pf"}], ")"}], - RowBox[{"1", "/", "3"}]]}], - SuperscriptBox["jMax", - RowBox[{"1", "/", "3"}]]]}]}], ",", - RowBox[{"t5", "\[Rule]", - RowBox[{"-", - FractionBox[ - RowBox[{ - SuperscriptBox[ - RowBox[{"(", - RowBox[{"-", - FractionBox["1", "2"]}], ")"}], - RowBox[{"1", "/", "3"}]], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - RowBox[{"-", "p0"}], "+", "pf"}], ")"}], - RowBox[{"1", "/", "3"}]]}], - SuperscriptBox["jMax", - RowBox[{"1", "/", "3"}]]]}]}], ",", - RowBox[{"t7", "\[Rule]", - RowBox[{"-", - FractionBox[ - RowBox[{ - SuperscriptBox[ - RowBox[{"(", - RowBox[{"-", - FractionBox["1", "2"]}], ")"}], - RowBox[{"1", "/", "3"}]], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - RowBox[{"-", "p0"}], "+", "pf"}], ")"}], - RowBox[{"1", "/", "3"}]]}], - SuperscriptBox["jMax", - RowBox[{"1", "/", "3"}]]]}]}]}], "}"}], ",", - RowBox[{"{", - RowBox[{ - RowBox[{"t1", "\[Rule]", - FractionBox[ - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - RowBox[{"-", "p0"}], "+", "pf"}], ")"}], - RowBox[{"1", "/", "3"}]], - RowBox[{ - SuperscriptBox["2", - RowBox[{"1", "/", "3"}]], " ", - SuperscriptBox["jMax", - RowBox[{"1", "/", "3"}]]}]]}], ",", - RowBox[{"t3", "\[Rule]", - FractionBox[ - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - RowBox[{"-", "p0"}], "+", "pf"}], ")"}], - RowBox[{"1", "/", "3"}]], - RowBox[{ - SuperscriptBox["2", - RowBox[{"1", "/", "3"}]], " ", - SuperscriptBox["jMax", - RowBox[{"1", "/", "3"}]]}]]}], ",", - RowBox[{"t5", "\[Rule]", - FractionBox[ - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - RowBox[{"-", "p0"}], "+", "pf"}], ")"}], - RowBox[{"1", "/", "3"}]], - RowBox[{ - SuperscriptBox["2", - RowBox[{"1", "/", "3"}]], " ", - SuperscriptBox["jMax", - RowBox[{"1", "/", "3"}]]}]]}], ",", - RowBox[{"t7", "\[Rule]", - FractionBox[ - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - RowBox[{"-", "p0"}], "+", "pf"}], ")"}], - RowBox[{"1", "/", "3"}]], - RowBox[{ - SuperscriptBox["2", - RowBox[{"1", "/", "3"}]], " ", - SuperscriptBox["jMax", - RowBox[{"1", "/", "3"}]]}]]}]}], "}"}], ",", - RowBox[{"{", - RowBox[{ - RowBox[{"t1", "\[Rule]", - FractionBox[ - RowBox[{ - SuperscriptBox[ - RowBox[{"(", - RowBox[{"-", "1"}], ")"}], - RowBox[{"2", "/", "3"}]], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - RowBox[{"-", "p0"}], "+", "pf"}], ")"}], - RowBox[{"1", "/", "3"}]]}], - RowBox[{ - SuperscriptBox["2", - RowBox[{"1", "/", "3"}]], " ", - SuperscriptBox["jMax", - RowBox[{"1", "/", "3"}]]}]]}], ",", - RowBox[{"t3", "\[Rule]", - FractionBox[ - RowBox[{ - SuperscriptBox[ - RowBox[{"(", - RowBox[{"-", "1"}], ")"}], - RowBox[{"2", "/", "3"}]], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - RowBox[{"-", "p0"}], "+", "pf"}], ")"}], - RowBox[{"1", "/", "3"}]]}], - RowBox[{ - SuperscriptBox["2", - RowBox[{"1", "/", "3"}]], " ", - SuperscriptBox["jMax", - RowBox[{"1", "/", "3"}]]}]]}], ",", - RowBox[{"t5", "\[Rule]", - FractionBox[ - RowBox[{ - SuperscriptBox[ - RowBox[{"(", - RowBox[{"-", "1"}], ")"}], - RowBox[{"2", "/", "3"}]], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - RowBox[{"-", "p0"}], "+", "pf"}], ")"}], - RowBox[{"1", "/", "3"}]]}], - RowBox[{ - SuperscriptBox["2", - RowBox[{"1", "/", "3"}]], " ", - SuperscriptBox["jMax", - RowBox[{"1", "/", "3"}]]}]]}], ",", - RowBox[{"t7", "\[Rule]", - FractionBox[ - RowBox[{ - SuperscriptBox[ - RowBox[{"(", - RowBox[{"-", "1"}], ")"}], - RowBox[{"2", "/", "3"}]], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - RowBox[{"-", "p0"}], "+", "pf"}], ")"}], - RowBox[{"1", "/", "3"}]]}], - RowBox[{ - SuperscriptBox["2", - RowBox[{"1", "/", "3"}]], " ", - SuperscriptBox["jMax", - RowBox[{"1", "/", "3"}]]}]]}]}], "}"}]}], "}"}]], "Output", - CellChangeTimes->{{3.8147688087061443`*^9, 3.814768898888846*^9}, - 3.814769136417777*^9, {3.8151616575983*^9, 3.815161669182846*^9}, { - 3.817471849892227*^9, 3.817471882353421*^9}}, - CellLabel-> - "Out[779]=",ExpressionUUID->"575ae277-55ba-4f5d-9bb7-4264a9c3c1a6"] -}, Open ]], - Cell["Brake Conditions", "Text", CellChangeTimes->{{3.815301060071776*^9, 3.815301062632262*^9}},ExpressionUUID->"f1f1d133-d9ee-48d8-ab08-\ @@ -618,10 +413,119 @@ Cell[BoxData[ 3.815387246688963*^9, {3.8153873822076607`*^9, 3.8153873870569553`*^9}}, CellLabel-> "Out[194]=",ExpressionUUID->"e39c0b7c-dbb9-40cb-82ec-435732f72361"] +}, Open ]], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{"Solve", "[", + RowBox[{ + RowBox[{"{", " ", + RowBox[{ + RowBox[{"v1", "\[Equal]", + RowBox[{"v0", "-", + RowBox[{"aMax", "*", "t1"}]}]}], ",", + RowBox[{ + RowBox[{"-", "vMax"}], "\[Equal]", + RowBox[{"v1", "-", + RowBox[{"aMax", "*", "t2"}], "+", + RowBox[{ + FractionBox["1", "2"], "jMax", " ", + SuperscriptBox["t2", "2"]}]}]}], ",", + RowBox[{"0", "\[Equal]", + RowBox[{ + RowBox[{"-", "aMax"}], "+", + RowBox[{"jMax", " ", "t2"}]}]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{"t1", ",", "t2", ",", "v1"}], "}"}]}], "]"}]], "Input", + CellChangeTimes->{{3.8175410738410997`*^9, 3.8175411736750193`*^9}, { + 3.817541259584454*^9, 3.8175413001474237`*^9}, {3.817541356210436*^9, + 3.8175413573686523`*^9}}, + CellLabel->"In[2]:=",ExpressionUUID->"2b4f4afd-c126-482c-a3f6-b488e5a0565c"], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{"{", + RowBox[{ + RowBox[{"t1", "\[Rule]", + FractionBox[ + RowBox[{ + RowBox[{"-", + SuperscriptBox["aMax", "2"]}], "+", + RowBox[{"2", " ", "jMax", " ", "v0"}], "+", + RowBox[{"2", " ", "jMax", " ", "vMax"}]}], + RowBox[{"2", " ", "aMax", " ", "jMax"}]]}], ",", + RowBox[{"t2", "\[Rule]", + FractionBox["aMax", "jMax"]}], ",", + RowBox[{"v1", "\[Rule]", + FractionBox[ + RowBox[{ + SuperscriptBox["aMax", "2"], "-", + RowBox[{"2", " ", "jMax", " ", "vMax"}]}], + RowBox[{"2", " ", "jMax"}]]}]}], "}"}], "}"}]], "Output", + CellChangeTimes->{{3.817541166642359*^9, 3.817541174125177*^9}, + 3.81754126032939*^9, 3.817541300882049*^9, {3.817541338945429*^9, + 3.817541357725025*^9}}, + CellLabel->"Out[2]=",ExpressionUUID->"e8c41703-c7c0-4861-af08-97504d931e3c"] +}, Open ]], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{ + RowBox[{"Solve", "[", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"a1", "\[Equal]", + RowBox[{"-", "aMax"}]}], ",", + RowBox[{"v1", "\[Equal]", + RowBox[{"v0", "-", + RowBox[{"aMax", "*", "t1"}]}]}], ",", + RowBox[{ + RowBox[{"-", "vMax"}], "\[Equal]", + RowBox[{"v1", "+", + RowBox[{"a1", "*", "t2"}], "+", + RowBox[{ + RowBox[{"jMax", "/", "2"}], "*", + RowBox[{"t2", "^", "2"}]}]}]}], ",", + RowBox[{"0", "\[Equal]", + RowBox[{"a1", "+", + RowBox[{"jMax", " ", "t2"}]}]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{"t1", ",", "t2", ",", "a1", ",", "v1"}], "}"}]}], "]"}], + "\n"}]], "Input", + CellChangeTimes->{{3.8175415349452047`*^9, 3.817541541413602*^9}}, + CellLabel->"In[3]:=",ExpressionUUID->"aeb71e0a-7391-4acf-aa84-dda309dee7aa"], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{"{", + RowBox[{ + RowBox[{"t1", "\[Rule]", + FractionBox[ + RowBox[{ + RowBox[{"-", + SuperscriptBox["aMax", "2"]}], "+", + RowBox[{"2", " ", "jMax", " ", "v0"}], "+", + RowBox[{"2", " ", "jMax", " ", "vMax"}]}], + RowBox[{"2", " ", "aMax", " ", "jMax"}]]}], ",", + RowBox[{"t2", "\[Rule]", + FractionBox["aMax", "jMax"]}], ",", + RowBox[{"a1", "\[Rule]", + RowBox[{"-", "aMax"}]}], ",", + RowBox[{"v1", "\[Rule]", + FractionBox[ + RowBox[{ + SuperscriptBox["aMax", "2"], "-", + RowBox[{"2", " ", "jMax", " ", "vMax"}]}], + RowBox[{"2", " ", "jMax"}]]}]}], "}"}], "}"}]], "Output", + CellChangeTimes->{3.817541541864256*^9}, + CellLabel->"Out[3]=",ExpressionUUID->"e2efd1a3-7c8f-453f-b25a-28d75b5ddd9e"] }, Open ]] }, -WindowSize->{931, 1027}, -WindowMargins->{{0, Automatic}, {Automatic, 0}}, +WindowSize->{931, 747}, +WindowMargins->{{-53, Automatic}, {Automatic, 68}}, FrontEndVersion->"12.1 for Mac OS X x86 (64-bit) (June 19, 2020)", StyleDefinitions->"Default.nb", ExpressionUUID->"706c30b1-c7eb-4050-b3e2-34c6f60d06c7" @@ -641,31 +545,35 @@ Cell[558, 20, 343, 5, 35, "Text",ExpressionUUID->"6747c71c-c66c-433d-9613-bddcf8 Cell[904, 27, 416, 8, 35, "Text",ExpressionUUID->"9c0ba946-2fb4-4a4b-a305-4d0102c9636e"], Cell[1323, 37, 3787, 118, 449, "Input",ExpressionUUID->"01d94644-43f4-4ebf-b9dd-650c0bcc93aa"], Cell[5113, 157, 875, 20, 30, "Input",ExpressionUUID->"ccac5833-2cc2-4703-8af2-4673d2c0a437"], +Cell[5991, 179, 158, 3, 35, "Text",ExpressionUUID->"f1f1d133-d9ee-48d8-ab08-fdb8b843aed9"], +Cell[6152, 184, 648, 18, 69, "Input",ExpressionUUID->"e9b3b786-2ee6-4ad4-90fe-0843f9a56019"], +Cell[CellGroupData[{ +Cell[6825, 206, 443, 10, 30, "Input",ExpressionUUID->"635600fe-01e0-4d25-8184-067440ee5de9"], +Cell[7271, 218, 976, 30, 58, "Output",ExpressionUUID->"778a3e8d-bcc9-4cbe-a256-9baed7c1507e"] +}, Open ]], Cell[CellGroupData[{ -Cell[6013, 181, 305, 5, 30, "Input",ExpressionUUID->"61a3a1a2-df88-4692-9d53-69048ae8b464"], -Cell[6321, 188, 5905, 193, 163, "Output",ExpressionUUID->"575ae277-55ba-4f5d-9bb7-4264a9c3c1a6"] +Cell[8284, 253, 400, 11, 30, "Input",ExpressionUUID->"75407e54-a918-40b0-94a0-d82634d676e5"], +Cell[8687, 266, 397, 11, 56, "Output",ExpressionUUID->"a84f9442-a454-4874-ae35-b8c5c8fbc207"] }, Open ]], -Cell[12241, 384, 158, 3, 35, "Text",ExpressionUUID->"f1f1d133-d9ee-48d8-ab08-fdb8b843aed9"], -Cell[12402, 389, 648, 18, 69, "Input",ExpressionUUID->"e9b3b786-2ee6-4ad4-90fe-0843f9a56019"], Cell[CellGroupData[{ -Cell[13075, 411, 443, 10, 30, "Input",ExpressionUUID->"635600fe-01e0-4d25-8184-067440ee5de9"], -Cell[13521, 423, 976, 30, 58, "Output",ExpressionUUID->"778a3e8d-bcc9-4cbe-a256-9baed7c1507e"] +Cell[9121, 282, 698, 17, 53, "Input",ExpressionUUID->"8efb60ef-a462-475b-8ff5-e51b2c982b76"], +Cell[9822, 301, 1127, 34, 57, "Output",ExpressionUUID->"f3046318-00c5-4e1d-967a-6f60d1eb4324"] }, Open ]], Cell[CellGroupData[{ -Cell[14534, 458, 400, 11, 30, "Input",ExpressionUUID->"75407e54-a918-40b0-94a0-d82634d676e5"], -Cell[14937, 471, 397, 11, 56, "Output",ExpressionUUID->"a84f9442-a454-4874-ae35-b8c5c8fbc207"] +Cell[10986, 340, 731, 21, 47, "Input",ExpressionUUID->"38121ff9-6236-4cfd-98df-aef065814608"], +Cell[11720, 363, 393, 11, 34, "Output",ExpressionUUID->"c3269006-4fb2-4435-a3e6-883665db4f7f"] }, Open ]], Cell[CellGroupData[{ -Cell[15371, 487, 698, 17, 53, "Input",ExpressionUUID->"8efb60ef-a462-475b-8ff5-e51b2c982b76"], -Cell[16072, 506, 1127, 34, 57, "Output",ExpressionUUID->"f3046318-00c5-4e1d-967a-6f60d1eb4324"] +Cell[12150, 379, 685, 17, 53, "Input",ExpressionUUID->"210a25f2-9b17-4e4b-bc3f-6f2e7a55ac8f"], +Cell[12838, 398, 713, 16, 56, "Output",ExpressionUUID->"e39c0b7c-dbb9-40cb-82ec-435732f72361"] }, Open ]], Cell[CellGroupData[{ -Cell[17236, 545, 731, 21, 47, "Input",ExpressionUUID->"38121ff9-6236-4cfd-98df-aef065814608"], -Cell[17970, 568, 393, 11, 34, "Output",ExpressionUUID->"c3269006-4fb2-4435-a3e6-883665db4f7f"] +Cell[13588, 419, 891, 24, 47, "Input",ExpressionUUID->"2b4f4afd-c126-482c-a3f6-b488e5a0565c"], +Cell[14482, 445, 877, 23, 56, "Output",ExpressionUUID->"e8c41703-c7c0-4861-af08-97504d931e3c"] }, Open ]], Cell[CellGroupData[{ -Cell[18400, 584, 685, 17, 53, "Input",ExpressionUUID->"210a25f2-9b17-4e4b-bc3f-6f2e7a55ac8f"], -Cell[19088, 603, 713, 16, 56, "Output",ExpressionUUID->"e39c0b7c-dbb9-40cb-82ec-435732f72361"] +Cell[15396, 473, 873, 25, 52, "Input",ExpressionUUID->"aeb71e0a-7391-4acf-aa84-dda309dee7aa"], +Cell[16272, 500, 822, 23, 104, "Output",ExpressionUUID->"e2efd1a3-7c8f-453f-b25a-28d75b5ddd9e"] }, Open ]] } ] diff --git a/notebooks/ruckig-step2.nb b/notebooks/ruckig-step2.nb index af57b592..99c061e6 100644 --- a/notebooks/ruckig-step2.nb +++ b/notebooks/ruckig-step2.nb @@ -10,10 +10,10 @@ NotebookFileLineBreakTest NotebookFileLineBreakTest NotebookDataPosition[ 158, 7] -NotebookDataLength[ 43288, 1139] -NotebookOptionsPosition[ 40713, 1091] -NotebookOutlinePosition[ 41135, 1108] -CellTagsIndexPosition[ 41092, 1105] +NotebookDataLength[ 66961, 1763] +NotebookOptionsPosition[ 63915, 1707] +NotebookOutlinePosition[ 64337, 1724] +CellTagsIndexPosition[ 64294, 1721] WindowFrame->Normal*) (* Beginning of Notebook Content *) @@ -42,7 +42,7 @@ Cell[BoxData[{ "\[IndentingNewLine]"}], "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"jerkProfile", "=", "jerkProfileUDUD"}], ";"}], + RowBox[{"jerkProfile", "=", "jerkProfileUDDU"}], ";"}], "\[IndentingNewLine]"}], "\[IndentingNewLine]", RowBox[{ RowBox[{"a1", "=", @@ -255,9 +255,10 @@ Cell[BoxData[{ 3.8171986249749393`*^9, {3.8172044839157457`*^9, 3.817204491762228*^9}, 3.817206432896442*^9, 3.817267327497333*^9, 3.817273066602022*^9, { 3.817472122381835*^9, 3.817472123717684*^9}, {3.81747221413058*^9, - 3.81747221438645*^9}, 3.8174915146800756`*^9}, + 3.81747221438645*^9}, 3.8174915146800756`*^9, 3.817530512234641*^9, + 3.817539624223989*^9, 3.817539757155711*^9}, CellLabel-> - "In[114]:=",ExpressionUUID->"9382707b-2876-460d-b0fb-ca70e05b1e9a"], + "In[363]:=",ExpressionUUID->"9382707b-2876-460d-b0fb-ca70e05b1e9a"], Cell[BoxData[ RowBox[{ @@ -268,7 +269,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"}], ",", @@ -285,7 +286,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"}], ",", @@ -322,7 +323,7 @@ Cell[BoxData[ RowBox[{ RowBox[{"(", RowBox[{ - RowBox[{"a7", "\[Equal]", "0"}], "/.", + RowBox[{"a7", "\[Equal]", "af"}], "/.", RowBox[{"{", RowBox[{"a0", "\[Rule]", "0"}], "}"}]}], ")"}], ",", RowBox[{ @@ -584,20 +585,21 @@ Cell[BoxData[ 3.817206428698655*^9, {3.8174722130924253`*^9, 3.817472217385104*^9}, { 3.8174747809412537`*^9, 3.817474786461072*^9}, {3.817490686824304*^9, 3.8174906869494677`*^9}, {3.817491156287717*^9, 3.817491162863855*^9}, { - 3.817491313811269*^9, 3.81749131393857*^9}}, + 3.817491313811269*^9, 3.81749131393857*^9}, {3.817539498083053*^9, + 3.817539499801177*^9}, {3.817539754038406*^9, 3.817539754218478*^9}}, CellLabel-> - "In[140]:=",ExpressionUUID->"93a49805-7b39-4e71-a5ce-78f2b77ae9c0"], + "In[389]:=",ExpressionUUID->"93a49805-7b39-4e71-a5ce-78f2b77ae9c0"], Cell["\<\ Information - Case 1a: Acc0_Acc1_Vel (UDDU) Solution 1 -- Case 1b: Acc0_Acc1_Vel (UDUD) Solution ? +- Case 1b: Acc0_Acc1_Vel (UDUD) Solution 1 - Case 2a: Acc0_Acc1 Solution 1 - Case 2b: Acc0_Acc1 (a0=0) Solution ? - Case 3a: Acc1_Vel (UDDU) - Case 3b: Acc1_Vel (UDUD) - Case 4: Acc0_Vel (UDDU), Root t5 -- Case 5: Vel +- Case 5: Vel, Root t1 - Case 8a: None - Case 8b: None Solution 1/2\ \>", "Text", @@ -607,8 +609,9 @@ Information 3.817124186878913*^9, 3.817124189865059*^9}, {3.817129328273472*^9, 3.8171293344972153`*^9}, {3.8171295373005*^9, 3.817129537356216*^9}, { 3.817474635933146*^9, 3.817474759233317*^9}, {3.817490334158193*^9, - 3.81749048191954*^9}, {3.817491196013754*^9, - 3.817491197517359*^9}},ExpressionUUID->"0616ae41-3a02-473a-878f-\ + 3.81749048191954*^9}, {3.817491196013754*^9, 3.817491197517359*^9}, { + 3.817530590294873*^9, 3.817530591542695*^9}, {3.817539682775482*^9, + 3.817539682839389*^9}},ExpressionUUID->"0616ae41-3a02-473a-878f-\ 96412e2fc8c5"], Cell["\<\ @@ -627,25 +630,124 @@ Cell[BoxData[{ RowBox[{"Simplify", "[", RowBox[{"resultT", "[", RowBox[{"[", - RowBox[{"7", ",", "1", ",", "5", ",", "2"}], "]"}], "]"}], + RowBox[{"4", ",", "1", ",", "1", ",", "2"}], "]"}], "]"}], "]"}]}], "\[IndentingNewLine]", RowBox[{ RowBox[{"ToString", "[", RowBox[{"tmp", ",", "CForm"}], "]"}], ";"}], "\[IndentingNewLine]", RowBox[{"CopyToClipboard", "[", "%", "]"}]}], "Input", CellChangeTimes->CompressedData[" -1:eJxTTMoPSmViYGAQBmIQfeTRnqfxBW8cpyy99QZEC/zI9EoA0n680sEg2slA +1:eJxTTMoPSmViYGAQA2IQfeTRnqfxBW8cpyy99QZEC/zI9EoA0n680sEg2slA w2sikD5X+TAARBv5dYeB6GaHXjDNe/lfEog+8PpOBojmsnlTD6LnzOBqA9FL sn74TgPSRbWioSDa7kje+hlAOj3xzA4QLbbw4yEQveryZzD9hrv3BIiuSOkD 0zmKv8+A+X1/wPS370ziu4D0vKssciBajStLzqHwjWNS/1dlEC1zyY7TE0S3 OYDp+93HRUB0bNBUURAdkzZbGUQbMbGqgOii97XaIHresXowvSS71RJEn7kq -YQOip2WYeYPoH30nfEA0AG0HmXw= +YQOip2WYeYPoH30nfED0Ps+p3xOAdD/Pv38gepdYPkcikD53lYUPRB85oCkB +on9YeoFpAJc0sPU= "], CellLabel-> - "In[1064]:=",ExpressionUUID->"f08feb2e-6079-473b-b866-92670dea0a56"], + "In[390]:=",ExpressionUUID->"f08feb2e-6079-473b-b866-92670dea0a56"], Cell[BoxData[ - FractionBox["aMax", "jMax"]], "Output", + RowBox[{ + RowBox[{"(", + RowBox[{"2", " ", "aMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "6"}], " ", + SuperscriptBox["aMax", "3"], " ", "tf"}], "-", + RowBox[{"3", " ", + SuperscriptBox["af", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"aMax", " ", "tf"}], "+", "v0", "-", "vf"}], ")"}]}], "-", + RowBox[{"6", " ", "af", " ", "aMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"aMax", " ", "tf"}], "+", "v0", "-", "vf"}], ")"}]}], "+", + RowBox[{ + SqrtBox["6"], " ", + RowBox[{"\[Sqrt]", + RowBox[{"(", + RowBox[{"aMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"6", " ", + SuperscriptBox["aMax", "5"], " ", + SuperscriptBox["tf", "2"]}], "+", + RowBox[{"3", " ", + SuperscriptBox["af", "4"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "p0"}], "-", + RowBox[{"2", " ", "pf"}], "+", + RowBox[{"aMax", " ", + SuperscriptBox["tf", "2"]}], "+", + RowBox[{"2", " ", "tf", " ", "v0"}]}], ")"}]}], "+", + RowBox[{"6", " ", + SuperscriptBox["af", "2"], " ", "aMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + SuperscriptBox["aMax", "2"], " ", + SuperscriptBox["tf", "2"]}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"]}], "+", + RowBox[{"aMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "4"}], " ", "p0"}], "+", + RowBox[{"4", " ", "pf"}], "+", + RowBox[{"tf", " ", "v0"}], "-", + RowBox[{"5", " ", "tf", " ", "vf"}]}], ")"}]}]}], ")"}]}], + "+", + RowBox[{"4", " ", + SuperscriptBox["af", "3"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", + SuperscriptBox["aMax", "2"], " ", + SuperscriptBox["tf", "2"]}], "+", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"], "+", + RowBox[{"2", " ", "aMax", " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf", "+", + RowBox[{"2", " ", "tf", " ", "v0"}], "-", + RowBox[{"tf", " ", "vf"}]}], ")"}]}]}], ")"}]}], "-", + RowBox[{"12", " ", "af", " ", + SuperscriptBox["aMax", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"]}], "+", + RowBox[{"aMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"4", " ", "p0"}], "-", + RowBox[{"4", " ", "pf"}], "+", + RowBox[{"tf", " ", "v0"}], "+", + RowBox[{"3", " ", "tf", " ", "vf"}]}], ")"}]}]}], ")"}]}]}], + ")"}]}], ")"}]}]}]}], ")"}]}], ")"}], "/", + RowBox[{"(", + RowBox[{"af", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"3", " ", + SuperscriptBox["af", "3"]}], "+", + RowBox[{"4", " ", + SuperscriptBox["af", "2"], " ", "aMax"}], "-", + RowBox[{"12", " ", "af", " ", + SuperscriptBox["aMax", "2"]}], "-", + RowBox[{"24", " ", + SuperscriptBox["aMax", "3"]}]}], ")"}]}], ")"}]}]], "Output", CellChangeTimes->{{3.817013038100574*^9, 3.8170130952655783`*^9}, { 3.817013843881184*^9, 3.817013913172606*^9}, 3.817013966856229*^9, { 3.8171141944209747`*^9, 3.817114247671588*^9}, 3.8171142928777733`*^9, { @@ -657,30 +759,28 @@ Cell[BoxData[ 3.817472288172709*^9}, 3.817490506492956*^9, {3.81749060009131*^9, 3.817490606780237*^9}, 3.8174907229493113`*^9, 3.8174907869144897`*^9, { 3.817490894583593*^9, 3.817490915801375*^9}, {3.817491035943132*^9, - 3.817491041107473*^9}}, + 3.817491041107473*^9}, {3.8175395210316668`*^9, 3.817539576386404*^9}, { + 3.817539653966858*^9, 3.8175396965766487`*^9}, 3.817539782239129*^9}, CellLabel-> - "Out[1061]=",ExpressionUUID->"b8016670-e79f-4cfb-88dd-c3de2f3ad4d1"] + "Out[390]=",ExpressionUUID->"fb0d3b69-6fc5-442b-8456-51f7f65e0c32"] }, Open ]], -Cell["\<\ -Print Roots -- Case 4, Root t5\ -\>", "Text", +Cell["Print Roots", "Text", CellChangeTimes->{{3.817491116483302*^9, 3.817491125196505*^9}, { - 3.817491200621257*^9, - 3.817491209965311*^9}},ExpressionUUID->"f1684154-b22e-4827-98da-\ + 3.817491200621257*^9, 3.817491209965311*^9}, {3.817534916325714*^9, + 3.81753491659593*^9}},ExpressionUUID->"f1684154-b22e-4827-98da-\ 4e4dad150835"], Cell[CellGroupData[{ Cell[BoxData[{ RowBox[{ - RowBox[{"tRoot", "=", "5"}], ";"}], "\[IndentingNewLine]", + RowBox[{"tRoot", "=", "1"}], ";"}], "\[IndentingNewLine]", RowBox[{ RowBox[{"tmpResult", "=", RowBox[{"resultT", "[", RowBox[{"[", - RowBox[{"7", ",", "1"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"8", ",", "1"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ RowBox[{"rootFunction", "=", RowBox[{"tmpResult", "[", @@ -697,87 +797,46 @@ Cell[BoxData[{ RowBox[{"rootFunction", "[", RowBox[{"[", "1", "]"}], "]"}], "[", "x", "]"}], ",", "x"}], "]"}], "[", - RowBox[{"[", "1", "]"}], "]"}], - RowBox[{ + RowBox[{"[", "5", "]"}], "]"}], + RowBox[{"Last", "[", RowBox[{"CoefficientList", "[", RowBox[{ RowBox[{ RowBox[{"rootFunction", "[", RowBox[{"[", "1", "]"}], "]"}], "[", "x", "]"}], ",", "x"}], "]"}], - "[", - RowBox[{"[", "5", "]"}], "]"}]], "]"}]}], "\[IndentingNewLine]", + "]"}]], "]"}]}], "\[IndentingNewLine]", RowBox[{ RowBox[{"ToString", "[", RowBox[{"tmp", ",", "CForm"}], "]"}], ";"}], "\[IndentingNewLine]", RowBox[{"CopyToClipboard", "[", "%", "]"}]}], "Input", CellChangeTimes->{{3.8174912269910707`*^9, 3.8174912369981537`*^9}, { 3.817491274208914*^9, 3.817491290374673*^9}, {3.8174913367940817`*^9, - 3.817491349584128*^9}, {3.817491538900215*^9, 3.817491568052608*^9}}, + 3.817491349584128*^9}, {3.817491538900215*^9, 3.817491568052608*^9}, { + 3.8175305970172663`*^9, 3.817530673061006*^9}}, CellLabel-> - "In[165]:=",ExpressionUUID->"217d7885-1018-483e-8952-08082feb5695"], + "In[252]:=",ExpressionUUID->"217d7885-1018-483e-8952-08082feb5695"], Cell[BoxData[ - RowBox[{ - FractionBox["1", - RowBox[{"12", " ", - SuperscriptBox["jMax", "4"]}]], - RowBox[{"(", - RowBox[{ - RowBox[{"3", " ", - SuperscriptBox["a0", "4"]}], "+", - RowBox[{"3", " ", - SuperscriptBox["af", "4"]}], "-", - RowBox[{"8", " ", - SuperscriptBox["a0", "3"], " ", "aMax"}], "-", - RowBox[{"4", " ", - SuperscriptBox["af", "3"], " ", "aMax"}], "+", - RowBox[{"6", " ", - SuperscriptBox["a0", "2"], " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["af", "2"], "+", - SuperscriptBox["aMax", "2"], "-", - RowBox[{"2", " ", "jMax", " ", "v0"}], "+", - RowBox[{"2", " ", "jMax", " ", "vf"}]}], ")"}]}], "-", - RowBox[{"12", " ", "a0", " ", "aMax", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["af", "2"], "+", - RowBox[{"2", " ", "jMax", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"-", "v0"}], "+", "vf"}], ")"}]}]}], ")"}]}], "+", - RowBox[{"6", " ", - SuperscriptBox["af", "2"], " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["aMax", "2"], "-", - RowBox[{"2", " ", "aMax", " ", "jMax", " ", "tf"}], "+", - RowBox[{"2", " ", "jMax", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"-", "v0"}], "+", "vf"}], ")"}]}]}], ")"}]}], "-", - RowBox[{"12", " ", "jMax", " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - SuperscriptBox["aMax", "2"], " ", - RowBox[{"(", - RowBox[{"v0", "-", "vf"}], ")"}]}], "-", - RowBox[{"jMax", " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{"v0", "-", "vf"}], ")"}], "2"]}], "+", - RowBox[{"2", " ", "aMax", " ", "jMax", " ", - RowBox[{"(", - RowBox[{"p0", "-", "pf", "+", - RowBox[{"tf", " ", "vf"}]}], ")"}]}]}], ")"}]}]}], - ")"}]}]], "Output", + FractionBox[ + RowBox[{ + RowBox[{"15", " ", + SuperscriptBox["a0", "2"]}], "+", + RowBox[{"16", " ", "a0", " ", "jMax", " ", "tf"}], "-", + RowBox[{"2", " ", + SuperscriptBox["jMax", "2"], " ", + SuperscriptBox["tf", "2"]}], "+", + RowBox[{"6", " ", "jMax", " ", "v0"}], "-", + RowBox[{"6", " ", "jMax", " ", "vf"}]}], + RowBox[{ + RowBox[{"4", " ", "a0", " ", "jMax"}], "+", + RowBox[{"4", " ", + SuperscriptBox["jMax", "2"], " ", "tf"}]}]]], "Output", 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.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}, CellLabel-> - "Out[168]=",ExpressionUUID->"619fb7f4-4bd5-4729-a718-7012dc0477cb"] + "Out[255]=",ExpressionUUID->"c8c1ef8b-557d-4acc-8854-09d94a878b1a"] }, Open ]], Cell[CellGroupData[{ @@ -799,18 +858,346 @@ Cell[BoxData[{ RowBox[{"tmp", ",", "CForm"}], "]"}], ";"}], "\[IndentingNewLine]", RowBox[{"CopyToClipboard", "[", "%", "]"}]}], "Input", CellChangeTimes->{{3.817491381329023*^9, 3.8174914182735243`*^9}, { - 3.817491588286084*^9, 3.817491628985776*^9}}, + 3.817491588286084*^9, 3.817491628985776*^9}, {3.8175306940387907`*^9, + 3.8175307010854073`*^9}, {3.817530733671631*^9, 3.817530733709938*^9}, { + 3.8175349306462812`*^9, 3.8175349353248568`*^9}, {3.817534969175642*^9, + 3.817534981662369*^9}}, CellLabel-> - "In[195]:=",ExpressionUUID->"58c30e25-83c4-4d05-ad21-e88fcb7f4181"], + "In[282]:=",ExpressionUUID->"58c30e25-83c4-4d05-ad21-e88fcb7f4181"], Cell[BoxData[ RowBox[{ - RowBox[{"-", - FractionBox["af", "jMax"]}], "+", "t"}]], "Output", + RowBox[{"(", + RowBox[{ + RowBox[{"12", " ", + SuperscriptBox["a0", "7"]}], "+", + RowBox[{ + SuperscriptBox["a0", "6"], " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"101", " ", "t"}], "+", + RowBox[{"19", " ", "tf"}]}], ")"}]}], "+", + RowBox[{"6", " ", + SuperscriptBox["a0", "5"], " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"45", " ", "jMax", " ", + SuperscriptBox["t", "2"]}], "+", + RowBox[{"22", " ", "jMax", " ", "t", " ", "tf"}], "-", + RowBox[{"5", " ", "jMax", " ", + SuperscriptBox["tf", "2"]}], "+", + RowBox[{"6", " ", "v0"}], "-", + RowBox[{"6", " ", "vf"}]}], ")"}]}], "+", + RowBox[{"6", " ", + SuperscriptBox["a0", "4"], " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"6", " ", "p0"}], "-", + RowBox[{"6", " ", "pf"}], "+", + RowBox[{"jMax", " ", "t", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"41", " ", + SuperscriptBox["t", "2"]}], "+", + RowBox[{"52", " ", "t", " ", "tf"}], "-", + RowBox[{"23", " ", + SuperscriptBox["tf", "2"]}]}], ")"}]}], "-", + RowBox[{"7", " ", "tf", " ", "v0"}], "+", + RowBox[{"11", " ", "t", " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}]}], "+", + RowBox[{"13", " ", "tf", " ", "vf"}]}], ")"}]}], "+", + RowBox[{"12", " ", + SuperscriptBox["a0", "3"], " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + SuperscriptBox["jMax", "2"], " ", + SuperscriptBox["t", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"6", " ", + SuperscriptBox["t", "2"]}], "+", + RowBox[{"22", " ", "t", " ", "tf"}], "-", + RowBox[{"11", " ", + SuperscriptBox["tf", "2"]}]}], ")"}]}], "-", + RowBox[{"8", " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"]}], "+", + RowBox[{"jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"16", " ", "p0", " ", "t"}], "-", + RowBox[{"16", " ", "pf", " ", "t"}], "-", + RowBox[{"4", " ", "p0", " ", "tf"}], "+", + RowBox[{"4", " ", "pf", " ", "tf"}], "-", + RowBox[{"25", " ", + SuperscriptBox["t", "2"], " ", "v0"}], "-", + RowBox[{"26", " ", "t", " ", "tf", " ", "v0"}], "+", + RowBox[{ + SuperscriptBox["tf", "2"], " ", "v0"}], "+", + RowBox[{"25", " ", + SuperscriptBox["t", "2"], " ", "vf"}], "+", + RowBox[{"42", " ", "t", " ", "tf", " ", "vf"}], "-", + RowBox[{"5", " ", + SuperscriptBox["tf", "2"], " ", "vf"}]}], ")"}]}]}], ")"}]}], "+", + + RowBox[{"144", " ", "a0", " ", + SuperscriptBox["jMax", "3"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "3"]}], "+", + RowBox[{"jMax", " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"p0", " ", + RowBox[{"(", + RowBox[{"t", "+", "tf"}], ")"}]}], "-", + RowBox[{"pf", " ", + RowBox[{"(", + RowBox[{"t", "+", "tf"}], ")"}]}], "-", + RowBox[{"5", " ", + SuperscriptBox["t", "2"], " ", "v0"}], "+", + RowBox[{ + SuperscriptBox["tf", "2"], " ", "v0"}], "+", + RowBox[{"5", " ", + SuperscriptBox["t", "2"], " ", "vf"}], "+", + RowBox[{"t", " ", "tf", " ", "vf"}]}], ")"}]}], "+", + RowBox[{ + SuperscriptBox["jMax", "2"], " ", "t", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"p0", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["t", "2"], "+", + RowBox[{"2", " ", "t", " ", "tf"}], "-", + SuperscriptBox["tf", "2"]}], ")"}]}], "+", + RowBox[{"pf", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + SuperscriptBox["t", "2"]}], "-", + RowBox[{"2", " ", "t", " ", "tf"}], "+", + SuperscriptBox["tf", "2"]}], ")"}]}], "-", + RowBox[{"t", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["t", "2"], "+", + RowBox[{"4", " ", "t", " ", "tf"}], "-", + RowBox[{"2", " ", + SuperscriptBox["tf", "2"]}]}], ")"}], " ", "v0"}], "+", + RowBox[{ + RowBox[{"(", + RowBox[{ + SuperscriptBox["t", "3"], "+", + RowBox[{"5", " ", + SuperscriptBox["t", "2"], " ", "tf"}], "-", + SuperscriptBox["tf", "3"]}], ")"}], " ", "vf"}]}], ")"}]}]}], + ")"}]}], "+", + RowBox[{"36", " ", + SuperscriptBox["a0", "2"], " ", + SuperscriptBox["jMax", "3"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + SuperscriptBox["jMax", "2"], " ", + SuperscriptBox["t", "3"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "t"}], "-", "tf"}], ")"}], " ", "tf"}], "-", + RowBox[{ + RowBox[{"(", + RowBox[{ + RowBox[{"19", " ", "t"}], "+", "tf"}], ")"}], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"]}], "-", + RowBox[{"jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"pf", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"9", " ", + SuperscriptBox["t", "2"]}], "+", + RowBox[{"2", " ", "t", " ", "tf"}], "-", + SuperscriptBox["tf", "2"]}], ")"}]}], "+", + RowBox[{"p0", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "9"}], " ", + SuperscriptBox["t", "2"]}], "-", + RowBox[{"2", " ", "t", " ", "tf"}], "+", + SuperscriptBox["tf", "2"]}], ")"}]}], "+", + RowBox[{"4", " ", "t", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"3", " ", "t"}], "-", "tf"}], ")"}], " ", + RowBox[{"(", + RowBox[{"t", "+", + RowBox[{"2", " ", "tf"}]}], ")"}], " ", "v0"}], "+", + RowBox[{ + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "12"}], " ", + SuperscriptBox["t", "3"]}], "-", + RowBox[{"29", " ", + SuperscriptBox["t", "2"], " ", "tf"}], "+", + RowBox[{"6", " ", "t", " ", + SuperscriptBox["tf", "2"]}], "+", + SuperscriptBox["tf", "3"]}], ")"}], " ", "vf"}]}], ")"}]}]}], + ")"}]}], "+", + RowBox[{"72", " ", + SuperscriptBox["jMax", "4"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + SuperscriptBox["jMax", "2"], " ", + SuperscriptBox["t", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "t"}], "-", "tf"}], ")"}], " ", "tf", " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf", "-", + RowBox[{"t", " ", "v0"}], "+", + RowBox[{ + RowBox[{"(", + RowBox[{"t", "+", "tf"}], ")"}], " ", "vf"}]}], ")"}]}], "+", + RowBox[{ + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "p0"}], "-", + RowBox[{"2", " ", "pf"}], "-", + RowBox[{"t", " ", "v0"}], "+", + RowBox[{"tf", " ", "v0"}], "+", + RowBox[{ + RowBox[{"(", + RowBox[{"t", "+", "tf"}], ")"}], " ", "vf"}]}], ")"}]}], "+", + RowBox[{"jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + SuperscriptBox["p0", "2"], " ", + RowBox[{"(", + RowBox[{"t", "-", "tf"}], ")"}]}], "+", + RowBox[{ + SuperscriptBox["pf", "2"], " ", + RowBox[{"(", + RowBox[{"t", "-", "tf"}], ")"}]}], "+", + RowBox[{"pf", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + SuperscriptBox["t", "2"]}], "-", + RowBox[{"2", " ", "t", " ", "tf"}], "+", + SuperscriptBox["tf", "2"]}], ")"}], " ", "v0"}], "+", + RowBox[{"t", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "3"}], " ", + SuperscriptBox["t", "2"]}], "+", + SuperscriptBox["tf", "2"]}], ")"}], " ", + SuperscriptBox["v0", "2"]}], "+", + RowBox[{"pf", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["t", "2"], "+", + SuperscriptBox["tf", "2"]}], ")"}], " ", "vf"}], "+", + RowBox[{ + RowBox[{"(", + RowBox[{ + RowBox[{"6", " ", + SuperscriptBox["t", "3"]}], "+", + RowBox[{ + SuperscriptBox["t", "2"], " ", "tf"}], "-", + SuperscriptBox["tf", "3"]}], ")"}], " ", "v0", " ", "vf"}], "-", + + RowBox[{ + SuperscriptBox["t", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"3", " ", "t"}], "+", "tf"}], ")"}], " ", + SuperscriptBox["vf", "2"]}], "-", + RowBox[{"p0", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "pf", " ", + RowBox[{"(", + RowBox[{"t", "-", "tf"}], ")"}]}], "-", + RowBox[{"2", " ", "t", " ", "tf", " ", "v0"}], "+", + RowBox[{ + SuperscriptBox["t", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "v0"}], "+", "vf"}], ")"}]}], "+", + RowBox[{ + SuperscriptBox["tf", "2"], " ", + RowBox[{"(", + RowBox[{"v0", "+", "vf"}], ")"}]}]}], ")"}]}]}], ")"}]}]}], + ")"}]}]}], ")"}], "/", + RowBox[{"(", + RowBox[{"jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + SuperscriptBox["a0", "6"]}], "+", + RowBox[{"6", " ", + SuperscriptBox["a0", "4"], " ", "jMax", " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}]}], "-", + RowBox[{"36", " ", + SuperscriptBox["a0", "2"], " ", + SuperscriptBox["jMax", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"]}], "+", + RowBox[{"48", " ", + SuperscriptBox["a0", "3"], " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf", "+", + RowBox[{"tf", " ", "vf"}]}], ")"}]}], "-", + RowBox[{"144", " ", "a0", " ", + SuperscriptBox["jMax", "3"], " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf", "+", + RowBox[{"tf", " ", "vf"}]}], ")"}]}], "+", + RowBox[{"72", " ", + SuperscriptBox["jMax", "3"], " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "3"], "+", + RowBox[{"jMax", " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"p0", "-", "pf", "+", + RowBox[{"tf", " ", "vf"}]}], ")"}], "2"]}]}], ")"}]}]}], ")"}]}], + ")"}]}]], "Output", CellChangeTimes->{{3.817491375709132*^9, 3.817491418809086*^9}, { - 3.817491592470599*^9, 3.817491629469158*^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}}, CellLabel-> - "Out[196]=",ExpressionUUID->"36913738-7027-4903-b562-7c6f319e74ef"] + "Out[283]=",ExpressionUUID->"480d0f52-423a-4af8-96f6-25e0827fcb2a"] }, Open ]], Cell["Case 9 - a3 != 0", "Text", @@ -1087,6 +1474,235 @@ vf)))))\"\>"], "Output", 3.817268097335195*^9, 3.817268131943392*^9}}, CellLabel-> "Out[601]=",ExpressionUUID->"9edd45ed-0ce4-41cc-9909-2527fddc394d"] +}, Open ]], + +Cell[CellGroupData[{ + +Cell[BoxData[{ + RowBox[{"tmp", "=", + RowBox[{ + RowBox[{"Simplify", "[", + RowBox[{"Solve", "[", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"(", + RowBox[{"p7", "//.", + RowBox[{"{", + RowBox[{ + RowBox[{"t2", "\[Rule]", "0"}], ",", + RowBox[{"t6", "\[Rule]", "0"}], ",", + RowBox[{"t1", "\[Rule]", "t"}], ",", + RowBox[{"t3", "\[Rule]", + RowBox[{ + FractionBox["a0", "jMax"], "+", "t"}]}], ",", + RowBox[{"t5", "\[Rule]", + SqrtBox[ + FractionBox[ + RowBox[{"vPlat", "-", "vf"}], "jMax"]]}], ",", + RowBox[{"t7", "\[Rule]", + SqrtBox[ + FractionBox[ + RowBox[{"vPlat", "-", "vf"}], "jMax"]]}]}], "}"}]}], ")"}], + "\[Equal]", "pf"}], "}"}], ",", + RowBox[{"{", "t4", "}"}]}], "]"}], "]"}], "[", + RowBox[{"[", + RowBox[{"1", ",", "1", ",", "2"}], "]"}], + "]"}]}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"ToString", "[", + RowBox[{"tmp", ",", "CForm"}], "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{"CopyToClipboard", "[", "%", "]"}], "\[IndentingNewLine]"}], "Input",\ + + CellChangeTimes->{{3.8175311793433113`*^9, 3.817531327589541*^9}, { + 3.817531409377803*^9, 3.8175314094721947`*^9}, {3.8175314452858133`*^9, + 3.817531474533996*^9}, {3.817531517634266*^9, 3.817531556211143*^9}, { + 3.8175316658581467`*^9, 3.817531669108927*^9}, {3.817532957786686*^9, + 3.817532996874202*^9}, {3.8175330516587467`*^9, 3.81753306961812*^9}, { + 3.817533169270577*^9, 3.817533170437025*^9}, {3.8175332090865507`*^9, + 3.817533258158308*^9}, {3.817535123681261*^9, 3.817535127470829*^9}}, + CellLabel-> + "In[289]:=",ExpressionUUID->"0d5d8425-f226-4e6a-91fd-34a5822f73b9"], + +Cell[BoxData[ + RowBox[{"-", + RowBox[{"(", + RowBox[{ + RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["a0", "3"], "+", + RowBox[{"3", " ", + SuperscriptBox["a0", "2"], " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "t"}], "+", + SqrtBox[ + FractionBox[ + RowBox[{ + RowBox[{"-", "vf"}], "+", "vPlat"}], "jMax"]]}], ")"}]}], "+", + RowBox[{"3", " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf", "+", + RowBox[{"jMax", " ", + SuperscriptBox["t", "3"]}], "+", + RowBox[{"2", " ", "t", " ", "v0"}], "+", + RowBox[{"2", " ", "jMax", " ", + SuperscriptBox["t", "2"], " ", + SqrtBox[ + FractionBox[ + RowBox[{ + RowBox[{"-", "vf"}], "+", "vPlat"}], "jMax"]]}], "+", + RowBox[{"2", " ", "v0", " ", + SqrtBox[ + FractionBox[ + RowBox[{ + RowBox[{"-", "vf"}], "+", "vPlat"}], "jMax"]]}], "+", + RowBox[{"vf", " ", + SqrtBox[ + FractionBox[ + RowBox[{ + RowBox[{"-", "vf"}], "+", "vPlat"}], "jMax"]]}], "-", + RowBox[{"vPlat", " ", + SqrtBox[ + FractionBox[ + RowBox[{ + RowBox[{"-", "vf"}], "+", "vPlat"}], "jMax"]]}]}], ")"}]}], + "+", + RowBox[{"3", " ", "a0", " ", "jMax", " ", + RowBox[{"(", + RowBox[{"v0", "+", + RowBox[{"jMax", " ", "t", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"3", " ", "t"}], "+", + RowBox[{"4", " ", + SqrtBox[ + FractionBox[ + RowBox[{ + RowBox[{"-", "vf"}], "+", "vPlat"}], "jMax"]]}]}], + ")"}]}]}], ")"}]}]}], ")"}]}], ")"}], "/", + RowBox[{"(", + RowBox[{"3", " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["a0", "2"], "+", + RowBox[{"4", " ", "a0", " ", "jMax", " ", "t"}], "+", + RowBox[{"2", " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"jMax", " ", + SuperscriptBox["t", "2"]}], "+", "v0"}], ")"}]}]}], ")"}]}], + ")"}]}], ")"}]}]], "Output", + CellChangeTimes->{{3.817531193319352*^9, 3.817531236762888*^9}, { + 3.817531272346353*^9, 3.817531327915782*^9}, 3.817531409884821*^9, { + 3.817531460644669*^9, 3.817531475170388*^9}, {3.8175315188349257`*^9, + 3.817531557043969*^9}, 3.817531670474701*^9, {3.8175329629092827`*^9, + 3.8175329974659023`*^9}, {3.817533055594714*^9, 3.817533070192478*^9}, + 3.817533172375989*^9, {3.8175332143676453`*^9, 3.817533258503757*^9}, { + 3.8175351242578382`*^9, 3.817535128442606*^9}}, + CellLabel-> + "Out[289]=",ExpressionUUID->"1dcd2096-117e-4fa7-8a27-82d21901823d"] +}, Open ]], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{"Solve", "[", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"(", + RowBox[{"p7", "/.", + RowBox[{"{", + RowBox[{ + RowBox[{"t2", "\[Rule]", "0"}], ",", + RowBox[{"t6", "\[Rule]", "0"}], ",", + RowBox[{"t1", "\[Rule]", "t"}], ",", + RowBox[{"t3", "\[Rule]", + RowBox[{ + FractionBox["a0", "jMax"], "+", "t"}]}]}], "}"}]}], ")"}], + "\[Equal]", "pf"}], "}"}], ",", + RowBox[{"{", "t4", "}"}]}], "]"}]], "Input", + CellChangeTimes->{{3.817531579989276*^9, 3.8175315988550177`*^9}, { + 3.81753166129846*^9, 3.817531661460588*^9}, {3.8175327812019587`*^9, + 3.817532800829281*^9}}, + CellLabel-> + "In[194]:=",ExpressionUUID->"4f3c940a-b41f-43f3-9aa8-8115c96b6bee"], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{"{", + RowBox[{"t4", "\[Rule]", + RowBox[{ + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "2"}], " ", + SuperscriptBox["a0", "3"]}], "-", + RowBox[{"6", " ", + SuperscriptBox["jMax", "2"], " ", "p0"}], "+", + RowBox[{"6", " ", + SuperscriptBox["jMax", "2"], " ", "pf"}], "-", + RowBox[{"12", " ", + SuperscriptBox["a0", "2"], " ", "jMax", " ", "t"}], "-", + RowBox[{"18", " ", "a0", " ", + SuperscriptBox["jMax", "2"], " ", + SuperscriptBox["t", "2"]}], "-", + RowBox[{"6", " ", + SuperscriptBox["jMax", "3"], " ", + SuperscriptBox["t", "3"]}], "-", + RowBox[{"3", " ", + SuperscriptBox["a0", "2"], " ", "jMax", " ", "t5"}], "-", + RowBox[{"12", " ", "a0", " ", + SuperscriptBox["jMax", "2"], " ", "t", " ", "t5"}], "-", + RowBox[{"6", " ", + SuperscriptBox["jMax", "3"], " ", + SuperscriptBox["t", "2"], " ", "t5"}], "+", + RowBox[{ + SuperscriptBox["jMax", "3"], " ", + SuperscriptBox["t5", "3"]}], "-", + RowBox[{"3", " ", + SuperscriptBox["a0", "2"], " ", "jMax", " ", "t7"}], "-", + RowBox[{"12", " ", "a0", " ", + SuperscriptBox["jMax", "2"], " ", "t", " ", "t7"}], "-", + RowBox[{"6", " ", + SuperscriptBox["jMax", "3"], " ", + SuperscriptBox["t", "2"], " ", "t7"}], "+", + RowBox[{"3", " ", + SuperscriptBox["jMax", "3"], " ", + SuperscriptBox["t5", "2"], " ", "t7"}], "+", + RowBox[{"3", " ", + SuperscriptBox["jMax", "3"], " ", "t5", " ", + SuperscriptBox["t7", "2"]}], "-", + RowBox[{ + SuperscriptBox["jMax", "3"], " ", + SuperscriptBox["t7", "3"]}], "-", + RowBox[{"6", " ", "a0", " ", "jMax", " ", "v0"}], "-", + RowBox[{"12", " ", + SuperscriptBox["jMax", "2"], " ", "t", " ", "v0"}], "-", + RowBox[{"6", " ", + SuperscriptBox["jMax", "2"], " ", "t5", " ", "v0"}], "-", + RowBox[{"6", " ", + SuperscriptBox["jMax", "2"], " ", "t7", " ", "v0"}]}], ")"}], "/", + RowBox[{"(", + RowBox[{"3", " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["a0", "2"], "+", + RowBox[{"4", " ", "a0", " ", "jMax", " ", "t"}], "+", + RowBox[{"2", " ", + SuperscriptBox["jMax", "2"], " ", + SuperscriptBox["t", "2"]}], "+", + RowBox[{"2", " ", "jMax", " ", "v0"}]}], ")"}]}], ")"}]}]}], "}"}], + "}"}]], "Output", + CellChangeTimes->{{3.817531592703692*^9, 3.817531617348398*^9}, { + 3.817531647389099*^9, 3.817531661813994*^9}, {3.817532788044551*^9, + 3.817532801189687*^9}, 3.817532948179289*^9}, + CellLabel-> + "Out[194]=",ExpressionUUID->"f8a67d4a-9073-4868-948b-275b14bd35f8"] }, Open ]] }, WindowSize->{904, 920}, @@ -1108,38 +1724,46 @@ CellTagsIndex->{} (*NotebookFileOutline Notebook[{ Cell[558, 20, 230, 4, 31, "Text",ExpressionUUID->"039b787c-64fd-4dfb-8011-578c383c402e"], -Cell[791, 26, 7460, 233, 822, "Input",ExpressionUUID->"9382707b-2876-460d-b0fb-ca70e05b1e9a"], -Cell[8254, 261, 12552, 327, 427, "Input",ExpressionUUID->"93a49805-7b39-4e71-a5ce-78f2b77ae9c0"], -Cell[20809, 590, 899, 21, 238, "Text",ExpressionUUID->"0616ae41-3a02-473a-878f-96412e2fc8c5"], -Cell[21711, 613, 284, 7, 52, "Text",ExpressionUUID->"407acd61-eed1-4280-b150-0da33c32906a"], +Cell[791, 26, 7530, 234, 822, "Input",ExpressionUUID->"9382707b-2876-460d-b0fb-ca70e05b1e9a"], +Cell[8324, 262, 12651, 328, 427, "Input",ExpressionUUID->"93a49805-7b39-4e71-a5ce-78f2b77ae9c0"], +Cell[20978, 592, 1003, 22, 238, "Text",ExpressionUUID->"0616ae41-3a02-473a-878f-96412e2fc8c5"], +Cell[21984, 616, 284, 7, 52, "Text",ExpressionUUID->"407acd61-eed1-4280-b150-0da33c32906a"], +Cell[CellGroupData[{ +Cell[22293, 627, 886, 21, 66, "Input",ExpressionUUID->"f08feb2e-6079-473b-b866-92670dea0a56"], +Cell[23182, 650, 4779, 114, 134, "Output",ExpressionUUID->"fb0d3b69-6fc5-442b-8456-51f7f65e0c32"] +}, Open ]], +Cell[27976, 767, 247, 4, 31, "Text",ExpressionUUID->"f1684154-b22e-4827-98da-4e4dad150835"], +Cell[CellGroupData[{ +Cell[28248, 775, 1508, 41, 144, "Input",ExpressionUUID->"217d7885-1018-483e-8952-08082feb5695"], +Cell[29759, 818, 864, 20, 54, "Output",ExpressionUUID->"c8c1ef8b-557d-4acc-8854-09d94a878b1a"] +}, Open ]], Cell[CellGroupData[{ -Cell[22020, 624, 838, 20, 66, "Input",ExpressionUUID->"f08feb2e-6079-473b-b866-92670dea0a56"], -Cell[22861, 646, 966, 15, 49, "Output",ExpressionUUID->"b8016670-e79f-4cfb-88dd-c3de2f3ad4d1"] +Cell[30660, 843, 945, 22, 85, "Input",ExpressionUUID->"58c30e25-83c4-4d05-ad21-e88fcb7f4181"], +Cell[31608, 867, 11921, 332, 299, "Output",ExpressionUUID->"480d0f52-423a-4af8-96f6-25e0827fcb2a"] }, Open ]], -Cell[23842, 664, 228, 7, 52, "Text",ExpressionUUID->"f1684154-b22e-4827-98da-4e4dad150835"], +Cell[43544, 1202, 160, 3, 31, "Text",ExpressionUUID->"5c00d19c-efbb-475f-a4a4-adc1877059e7"], +Cell[43707, 1207, 5502, 128, 275, "Input",ExpressionUUID->"013ba19d-162c-4e14-9e6b-43bcf125ceef"], +Cell[49212, 1337, 531, 9, 31, "Text",ExpressionUUID->"ccf44efd-e09a-4bbb-a696-b4c90f7a95eb"], Cell[CellGroupData[{ -Cell[24095, 675, 1480, 41, 144, "Input",ExpressionUUID->"217d7885-1018-483e-8952-08082feb5695"], -Cell[25578, 718, 2153, 61, 75, "Output",ExpressionUUID->"619fb7f4-4bd5-4729-a718-7012dc0477cb"] +Cell[49768, 1350, 948, 18, 47, "Input",ExpressionUUID->"b4fd3303-7c30-4af8-94b1-4505577eaf40"], +Cell[50719, 1370, 636, 10, 31, "Output",ExpressionUUID->"3c247a78-5f1f-4bfe-aeb8-c0def9d99fe4"] }, Open ]], +Cell[51370, 1383, 182, 3, 31, "Text",ExpressionUUID->"6e1bb727-a1dc-4c24-af7f-abfa0f3b9409"], Cell[CellGroupData[{ -Cell[27768, 784, 744, 19, 85, "Input",ExpressionUUID->"58c30e25-83c4-4d05-ad21-e88fcb7f4181"], -Cell[28515, 805, 292, 7, 50, "Output",ExpressionUUID->"36913738-7027-4903-b562-7c6f319e74ef"] +Cell[51577, 1390, 952, 24, 66, "Input",ExpressionUUID->"c24549d5-e55a-4ad4-a91d-4e532e8d2688"], +Cell[52532, 1416, 781, 12, 108, "Output",ExpressionUUID->"8a1b58c3-1441-4fe3-bbe7-d56171dd7cae"] }, Open ]], -Cell[28822, 815, 160, 3, 31, "Text",ExpressionUUID->"5c00d19c-efbb-475f-a4a4-adc1877059e7"], -Cell[28985, 820, 5502, 128, 275, "Input",ExpressionUUID->"013ba19d-162c-4e14-9e6b-43bcf125ceef"], -Cell[34490, 950, 531, 9, 31, "Text",ExpressionUUID->"ccf44efd-e09a-4bbb-a696-b4c90f7a95eb"], Cell[CellGroupData[{ -Cell[35046, 963, 948, 18, 47, "Input",ExpressionUUID->"b4fd3303-7c30-4af8-94b1-4505577eaf40"], -Cell[35997, 983, 636, 10, 31, "Output",ExpressionUUID->"3c247a78-5f1f-4bfe-aeb8-c0def9d99fe4"] +Cell[53350, 1433, 812, 21, 66, "Input",ExpressionUUID->"27add3ed-17eb-49e9-9b82-a98025fa047d"], +Cell[54165, 1456, 1254, 19, 203, "Output",ExpressionUUID->"9edd45ed-0ce4-41cc-9909-2527fddc394d"] }, Open ]], -Cell[36648, 996, 182, 3, 31, "Text",ExpressionUUID->"6e1bb727-a1dc-4c24-af7f-abfa0f3b9409"], Cell[CellGroupData[{ -Cell[36855, 1003, 952, 24, 66, "Input",ExpressionUUID->"c24549d5-e55a-4ad4-a91d-4e532e8d2688"], -Cell[37810, 1029, 781, 12, 108, "Output",ExpressionUUID->"8a1b58c3-1441-4fe3-bbe7-d56171dd7cae"] +Cell[55456, 1480, 1812, 44, 156, "Input",ExpressionUUID->"0d5d8425-f226-4e6a-91fd-34a5822f73b9"], +Cell[57271, 1526, 2994, 80, 179, "Output",ExpressionUUID->"1dcd2096-117e-4fa7-8a27-82d21901823d"] }, Open ]], Cell[CellGroupData[{ -Cell[38628, 1046, 812, 21, 66, "Input",ExpressionUUID->"27add3ed-17eb-49e9-9b82-a98025fa047d"], -Cell[39443, 1069, 1254, 19, 203, "Output",ExpressionUUID->"9edd45ed-0ce4-41cc-9909-2527fddc394d"] +Cell[60302, 1611, 764, 21, 46, "Input",ExpressionUUID->"4f3c940a-b41f-43f3-9aa8-8115c96b6bee"], +Cell[61069, 1634, 2830, 70, 76, "Output",ExpressionUUID->"f8a67d4a-9073-4868-948b-275b14bd35f8"] }, Open ]] } ] diff --git a/src/brake.cpp b/src/brake.cpp index d6e4e524..efe7c97e 100644 --- a/src/brake.cpp +++ b/src/brake.cpp @@ -41,15 +41,13 @@ void RuckigStep1::get_brake_trajectory(double v0, double a0, double vMax, double double t_to_v_max = -(-a0 - std::sqrt(std::pow(a0,2) + 2 * jMax * v0 - 2 * jMax * vMax))/jMax; double t_to_v_max_brake_for_other = (2 * a0 + std::sqrt(2) * std::sqrt(std::pow(a0, 2) + 2 * jMax * (v0 + vMax)))/(2 * jMax); - // std::cout << t_to_other_a_max << " " << t_to_v_max << " " << t_to_v_max_brake_for_other << std::endl; - if (t_to_v_max_brake_for_other < t_to_other_a_max && t_to_v_max_brake_for_other < t_to_v_max) { t_brake[0] = t_to_v_max_brake_for_other - 2e-15; } else if (t_to_other_a_max < t_to_v_max) { double v_at_a_other_a_max = v_at_t(v0, a0, -jMax, t_to_other_a_max); double t_to_v_max_while_a_max = (v_at_a_other_a_max - vMax)/aMax; - double t_to_v_max_brake_for_other_a_max = -(std::pow(aMax, 2) - 2 * jMax * (v_at_a_max + vMax))/(2 * aMax * jMax); + double t_to_v_max_brake_for_other_a_max = -(std::pow(aMax, 2) - 2 * jMax * (v_at_a_other_a_max + vMax))/(2 * aMax * jMax); t_brake[0] = t_to_other_a_max; t_brake[1] = std::min(t_to_v_max_while_a_max, t_to_v_max_brake_for_other_a_max); @@ -78,7 +76,6 @@ void RuckigStep1::get_brake_trajectory(double v0, double a0, double vMax, double double t_to_v_max_while_a_max = (v_at_a_max - vMax)/aMax; double t_to_v_max_in_reverse_j_direction = (-std::pow(aMax, 2) + 2 * jMax * v_at_a_max + 2 * jMax * vMax)/(2*aMax*jMax); t_brake[1] = std::min(t_to_v_max_while_a_max, t_to_v_max_in_reverse_j_direction); - // {a1 == -aMax, v1 == v0 - aMax * t1, -vMax == v1 + a1 * t2 + jMax / 2 * t2^2, 0 == a1 + jMax t2}, {t1, t2, a1, v1} } else if (v_at_a_max < -vMax) { double t_to_other_a_max = -(a0 - aMax) / jMax - 2e-15; @@ -87,7 +84,7 @@ void RuckigStep1::get_brake_trajectory(double v0, double a0, double vMax, double if (t_to_other_a_max < t_to_v_max) { double v_at_a_other_a_max = v_at_t(v0, a0, jMax, t_to_other_a_max); double t_to_v_max_while_a_max = -(v_at_a_other_a_max + vMax)/aMax; - double t_to_v_max_brake_for_other_a_max = -(std::pow(aMax, 2) + 2 * jMax * (v_at_a_max - vMax))/(2 * aMax * jMax); + double t_to_v_max_brake_for_other_a_max = -(std::pow(aMax, 2) + 2 * jMax * (v_at_a_other_a_max - vMax))/(2 * aMax * jMax); t_brake[0] = t_to_other_a_max; t_brake[1] = std::min(t_to_v_max_while_a_max, t_to_v_max_brake_for_other_a_max); diff --git a/src/profile.cpp b/src/profile.cpp index d079ff2e..a7ac4203 100644 --- a/src/profile.cpp +++ b/src/profile.cpp @@ -23,7 +23,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 beginnging if the initial velocity and acceleration are too high + // 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; 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; }) @@ -31,6 +31,11 @@ bool Profile::check(double pf, double vf, double af, double vMax, double aMax) c && std::abs(p[7] - pf) < 1e-8 && std::abs(v[7] - vf) < 1e-8 && std::abs(a[7] - af) < 1e-8; } +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); +} + std::tuple Profile::integrate(double t, double p0, double v0, double a0, double j) { const double p_new = p0 + t * (v0 + t * (a0 / 2 + t * j / 6)); const double v_new = v0 + t * (a0 + t * j / 2); diff --git a/src/step1.cpp b/src/step1.cpp index 1ab6e1bc..99856bee 100644 --- a/src/step1.cpp +++ b/src/step1.cpp @@ -7,11 +7,11 @@ namespace ruckig { -RuckigStep1::RuckigStep1(double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { +RuckigStep1::RuckigStep1(double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax): p0(p0), v0(v0), a0(a0), pf(pf), vf(vf), af(af) { } -bool RuckigStep1::time_up_acc0_acc1_vel(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { +bool RuckigStep1::time_up_acc0_acc1_vel(Profile& profile, double vMax, double aMax, double jMax) { double a0_a0 = a0 * a0; double aMax_aMax = aMax * aMax; @@ -27,7 +27,7 @@ bool RuckigStep1::time_up_acc0_acc1_vel(Profile& profile, double p0, double v0, return profile.check(pf, vf, af, vMax, aMax); } -bool RuckigStep1::time_up_acc1_vel(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { +bool RuckigStep1::time_up_acc1_vel(Profile& profile, double vMax, double aMax, double jMax) { profile.t[0] = (-2*a0*jMax + Sqrt(2)*Sqrt(Power(a0,2) + 2*jMax*(-v0 + vMax))*Abs(jMax))/(2*Power(jMax,2)); profile.t[1] = 0; profile.t[2] = Sqrt(Power(a0,2)/2 + jMax*(-v0 + vMax))/Abs(jMax); @@ -40,7 +40,7 @@ bool RuckigStep1::time_up_acc1_vel(Profile& profile, double p0, double v0, doubl return profile.check(pf, vf, af, vMax, aMax); } -bool RuckigStep1::time_up_acc0_vel(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { +bool RuckigStep1::time_up_acc0_vel(Profile& profile, double vMax, double aMax, double jMax) { // Solution 2 if (jMax > 0) { profile.t[0] = (-a0 + aMax)/jMax; @@ -68,7 +68,7 @@ bool RuckigStep1::time_up_acc0_vel(Profile& profile, double p0, double v0, doubl return profile.check(pf, vf, af, vMax, aMax); } -bool RuckigStep1::time_up_vel(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { +bool RuckigStep1::time_up_vel(Profile& profile, double vMax, double aMax, double jMax) { // Solution 4 if (jMax > 0) { profile.t[0] = ((-2*a0*jMax + Sqrt(2)*Sqrt(Power(a0,2) + 2*jMax*(-v0 + vMax))*Abs(jMax))/(2*Power(jMax,2))); @@ -96,7 +96,7 @@ bool RuckigStep1::time_up_vel(Profile& profile, double p0, double v0, double a0, return profile.check(pf, vf, af, vMax, aMax); } -bool RuckigStep1::time_up_acc0_acc1(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { +bool RuckigStep1::time_up_acc0_acc1(Profile& profile, double vMax, double aMax, double jMax) { const double h1 = Sqrt(6)*Sqrt(Power(aMax,2)*(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) + 6*(Power(aMax,4) + 4*aMax*Power(jMax,2)*(-p0 + pf) - 2*Power(aMax,2)*jMax*(v0 + vf) + 2*Power(jMax,2)*(Power(v0,2) + Power(vf,2)))))*Abs(jMax); profile.t[0] = (-a0 + aMax)/jMax; @@ -111,7 +111,7 @@ bool RuckigStep1::time_up_acc0_acc1(Profile& profile, double p0, double v0, doub return profile.check(pf, vf, af, vMax, aMax); } -bool RuckigStep1::time_up_acc1(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { +bool RuckigStep1::time_up_acc1(Profile& profile, double vMax, double aMax, double jMax) { std::array polynom; polynom[0] = 1.0; polynom[1] = (2*(2*a0 + aMax))/jMax; @@ -144,7 +144,7 @@ bool RuckigStep1::time_up_acc1(Profile& profile, double p0, double v0, double a0 return false; } -bool RuckigStep1::time_up_acc0(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { +bool RuckigStep1::time_up_acc0(Profile& profile, double vMax, double aMax, double jMax) { std::array polynom; polynom[0] = 1.0; polynom[1] = (2*aMax)/jMax; @@ -177,7 +177,7 @@ bool RuckigStep1::time_up_acc0(Profile& profile, double p0, double v0, double a0 return false; } -bool RuckigStep1::time_up_none(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { +bool RuckigStep1::time_up_none(Profile& profile, double vMax, double aMax, double jMax) { if (std::abs(v0) < DBL_EPSILON && std::abs(a0) < DBL_EPSILON && std::abs(vf) < DBL_EPSILON && std::abs(af) < DBL_EPSILON) { profile.t[0] = std::cbrt((pf - p0)/(2*jMax)); profile.t[1] = 0; @@ -205,7 +205,7 @@ bool RuckigStep1::time_up_none(Profile& profile, double p0, double v0, double a0 } // Refine root - if (std::abs(Roots::polyEval(polynom, t)) > 1e-7) { + if (std::abs(Roots::polyEval(polynom, t)) > 1e-9) { t = Roots::shrinkInterval(polynom, t - 1e-5, t + 1e-5, 1e-14); } @@ -228,87 +228,87 @@ bool RuckigStep1::time_up_none(Profile& profile, double p0, double v0, double a0 return false; } -bool RuckigStep1::time_down_acc0_acc1_vel(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { - return time_up_acc0_acc1_vel(profile, p0, v0, a0, pf, vf, af, -vMax, -aMax, -jMax); +bool RuckigStep1::time_down_acc0_acc1_vel(Profile& profile, double vMax, double aMax, double jMax) { + return time_up_acc0_acc1_vel(profile, -vMax, -aMax, -jMax); } -bool RuckigStep1::time_down_acc1_vel(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { - return time_up_acc1_vel(profile, p0, v0, a0, pf, vf, af, -vMax, -aMax, -jMax); +bool RuckigStep1::time_down_acc1_vel(Profile& profile, double vMax, double aMax, double jMax) { + return time_up_acc1_vel(profile, -vMax, -aMax, -jMax); } -bool RuckigStep1::time_down_acc0_vel(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { - return time_up_acc0_vel(profile, p0, v0, a0, pf, vf, af, -vMax, -aMax, -jMax); +bool RuckigStep1::time_down_acc0_vel(Profile& profile, double vMax, double aMax, double jMax) { + return time_up_acc0_vel(profile, -vMax, -aMax, -jMax); } -bool RuckigStep1::time_down_vel(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { - return time_up_vel(profile, p0, v0, a0, pf, vf, af, -vMax, -aMax, -jMax); +bool RuckigStep1::time_down_vel(Profile& profile, double vMax, double aMax, double jMax) { + return time_up_vel(profile, -vMax, -aMax, -jMax); } -bool RuckigStep1::time_down_acc0_acc1(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { - return time_up_acc0_acc1(profile, p0, v0, a0, pf, vf, af, -vMax, -aMax, -jMax); +bool RuckigStep1::time_down_acc0_acc1(Profile& profile, double vMax, double aMax, double jMax) { + return time_up_acc0_acc1(profile, -vMax, -aMax, -jMax); } -bool RuckigStep1::time_down_acc1(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { - return time_up_acc1(profile, p0, v0, a0, pf, vf, af, -vMax, -aMax, -jMax); +bool RuckigStep1::time_down_acc1(Profile& profile, double vMax, double aMax, double jMax) { + return time_up_acc1(profile, -vMax, -aMax, -jMax); } -bool RuckigStep1::time_down_acc0(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { - return time_up_acc0(profile, p0, v0, a0, pf, vf, af, -vMax, -aMax, -jMax); +bool RuckigStep1::time_down_acc0(Profile& profile, double vMax, double aMax, double jMax) { + return time_up_acc0(profile, -vMax, -aMax, -jMax); } -bool RuckigStep1::time_down_none(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { - return time_up_none(profile, p0, v0, a0, pf, vf, af, -vMax, -aMax, -jMax); +bool RuckigStep1::time_down_none(Profile& profile, double vMax, double aMax, double jMax) { + return time_up_none(profile, -vMax, -aMax, -jMax); } -bool RuckigStep1::get_profile(Profile& profile, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { +bool RuckigStep1::get_profile(Profile& profile, double vMax, double aMax, double jMax) { // Test all cases to get ones that match if (pf > p0) { - if (time_up_acc0_acc1_vel(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + if (time_up_acc0_acc1_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC0_ACC1_VEL; - } else if (time_down_acc0_acc1_vel(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_acc0_acc1_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC0_ACC1_VEL; - } else if (time_up_acc1_vel(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_up_acc1_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC1_VEL; - } else if (time_down_acc1_vel(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_acc1_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC1_VEL; - } else if (time_up_acc0_vel(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_up_acc0_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC0_VEL; - } else if (time_down_acc0_vel(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_acc0_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC0_VEL; - } else if (time_up_vel(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_up_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_VEL; - } else if (time_down_vel(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_VEL; - } else if (time_up_none(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_up_none(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_NONE; - } else if (time_up_acc0(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_up_acc0(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC0; - } else if (time_up_acc1(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_up_acc1(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC1; - } else if (time_up_acc0_acc1(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_up_acc0_acc1(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC0_ACC1; - } else if (time_down_none(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_none(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_NONE; - } else if (time_down_acc0(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_acc0(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC0; - } else if (time_down_acc1(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_acc1(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC1; - } else if (time_down_acc0_acc1(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_acc0_acc1(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC0_ACC1; } else { @@ -316,52 +316,52 @@ bool RuckigStep1::get_profile(Profile& profile, double p0, double v0, double a0, } } else { - if (time_down_acc0_acc1_vel(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + if (time_down_acc0_acc1_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC0_ACC1_VEL; - } else if (time_up_acc0_acc1_vel(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_up_acc0_acc1_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC0_ACC1_VEL; - } else if (time_down_acc1_vel(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_acc1_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC1_VEL; - } else if (time_up_acc1_vel(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_up_acc1_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC1_VEL; - } else if (time_down_acc0_vel(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_acc0_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC0_VEL; - } else if (time_up_acc0_vel(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_up_acc0_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC0_VEL; - } else if (time_down_vel(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_VEL; - } else if (time_up_vel(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_up_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_VEL; - } else if (time_down_none(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_none(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_NONE; - } else if (time_down_acc0(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_acc0(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC0; - } else if (time_down_acc1(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_acc1(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC1; - } else if (time_down_acc0_acc1(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_acc0_acc1(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC0_ACC1; - } else if (time_up_none(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_up_none(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_NONE; - } else if (time_up_acc0(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_up_acc0(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC0; - } else if (time_up_acc1(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_up_acc1(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC1; - } else if (time_up_acc0_acc1(profile, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_up_acc0_acc1(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC0_ACC1; } else { diff --git a/src/step2.cpp b/src/step2.cpp index a36c50a5..a41b7f64 100644 --- a/src/step2.cpp +++ b/src/step2.cpp @@ -7,23 +7,23 @@ namespace ruckig { -RuckigStep2::RuckigStep2(double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { +RuckigStep2::RuckigStep2(double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax): tf(tf), p0(p0), v0(v0), a0(a0), pf(pf), vf(vf), af(af) { } -bool RuckigStep2::time_up_acc0_acc1_vel(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { +bool RuckigStep2::time_up_acc0_acc1_vel(Profile& profile, double vMax, double aMax, double jMax) { // Profile UDDU { profile.t[0] = (-a0 + aMax)/jMax; - profile.t[1] = (3*aMax*jMax*(Power(a0,2) + 2*a0*aMax - 6*Power(aMax,2) + 2*aMax*jMax*tf + 2*jMax*(-v0 + vf)) - Sqrt(3)*Sqrt(Power(aMax,2)*(-3*Power(a0,4) + 4*Power(a0,3)*aMax + 12*Power(a0,2)*(Power(aMax,2) - aMax*jMax*tf + jMax*(v0 - vf)) - 24*a0*aMax*(Power(aMax,2) - aMax*jMax*tf + jMax*(v0 - vf)) + 12*(Power(aMax,4) - 2*Power(aMax,3)*jMax*tf + Power(aMax,2)*Power(jMax,2)*Power(tf,2) - Power(jMax,2)*Power(v0 - vf,2) + 2*aMax*Power(jMax,2)*(2*p0 - 2*pf + tf*(v0 + vf)))))*Abs(jMax))/(12.*Power(aMax,2)*Power(jMax,2)); + profile.t[1] = -(-3*Power(a0,2)*aMax*jMax + 3*Power(af,2)*aMax*jMax - 6*a0*Power(aMax,2)*jMax + 6*af*Power(aMax,2)*jMax + 18*Power(aMax,3)*jMax - 6*Power(aMax,2)*Power(jMax,2)*tf + 6*aMax*Power(jMax,2)*v0 - 6*aMax*Power(jMax,2)*vf + Sqrt(3)*Sqrt(Power(aMax,2)*Power(jMax,2)*(-3*Power(a0,4) - 3*Power(af,4) + 4*Power(a0,3)*aMax - 4*Power(af,3)*aMax + 6*Power(a0,2)*(Power(af,2) + 2*af*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*(v0 - vf))) - 12*a0*aMax*(Power(af,2) + 2*af*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*(v0 - vf))) + 12*Power(af,2)*(Power(aMax,2) - aMax*jMax*tf + jMax*(-v0 + vf)) + 24*af*aMax*(Power(aMax,2) - aMax*jMax*tf + jMax*(-v0 + vf)) + 12*(Power(aMax,4) - 2*Power(aMax,3)*jMax*tf + Power(aMax,2)*Power(jMax,2)*Power(tf,2) - Power(jMax,2)*Power(v0 - vf,2) + 2*aMax*Power(jMax,2)*(2*p0 - 2*pf + tf*(v0 + vf))))))/(12.*Power(aMax,2)*Power(jMax,2)); profile.t[2] = aMax/jMax; - profile.t[3] = (-6*Power(aMax,3)*jMax + Sqrt(3)*Sqrt(Power(aMax,2)*(-3*Power(a0,4) + 4*Power(a0,3)*aMax + 12*Power(a0,2)*(Power(aMax,2) - aMax*jMax*tf + jMax*(v0 - vf)) - 24*a0*aMax*(Power(aMax,2) - aMax*jMax*tf + jMax*(v0 - vf)) + 12*(Power(aMax,4) - 2*Power(aMax,3)*jMax*tf + Power(aMax,2)*Power(jMax,2)*Power(tf,2) - Power(jMax,2)*Power(v0 - vf,2) + 2*aMax*Power(jMax,2)*(2*p0 - 2*pf + tf*(v0 + vf)))))*Abs(jMax))/(6.*Power(aMax,2)*Power(jMax,2)); + profile.t[3] = (-6*Power(aMax,3)*jMax + Sqrt(3)*Sqrt(Power(aMax,2)*Power(jMax,2)*(-3*Power(a0,4) - 3*Power(af,4) + 4*Power(a0,3)*aMax - 4*Power(af,3)*aMax + 6*Power(a0,2)*(Power(af,2) + 2*af*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*(v0 - vf))) - 12*a0*aMax*(Power(af,2) + 2*af*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*(v0 - vf))) + 12*Power(af,2)*(Power(aMax,2) - aMax*jMax*tf + jMax*(-v0 + vf)) + 24*af*aMax*(Power(aMax,2) - aMax*jMax*tf + jMax*(-v0 + vf)) + 12*(Power(aMax,4) - 2*Power(aMax,3)*jMax*tf + Power(aMax,2)*Power(jMax,2)*Power(tf,2) - Power(jMax,2)*Power(v0 - vf,2) + 2*aMax*Power(jMax,2)*(2*p0 - 2*pf + tf*(v0 + vf))))))/(6.*Power(aMax,2)*Power(jMax,2)); profile.t[4] = profile.t[2]; - profile.t[5] = -(3*aMax*jMax*(Power(a0,2) - 2*a0*aMax + 6*Power(aMax,2) - 2*aMax*jMax*tf + 2*jMax*(-v0 + vf)) + Sqrt(3)*Sqrt(Power(aMax,2)*(-3*Power(a0,4) + 4*Power(a0,3)*aMax + 12*Power(a0,2)*(Power(aMax,2) - aMax*jMax*tf + jMax*(v0 - vf)) - 24*a0*aMax*(Power(aMax,2) - aMax*jMax*tf + jMax*(v0 - vf)) + 12*(Power(aMax,4) - 2*Power(aMax,3)*jMax*tf + Power(aMax,2)*Power(jMax,2)*Power(tf,2) - Power(jMax,2)*Power(v0 - vf,2) + 2*aMax*Power(jMax,2)*(2*p0 - 2*pf + tf*(v0 + vf)))))*Abs(jMax))/(12.*Power(aMax,2)*Power(jMax,2)); - profile.t[6] = profile.t[2]; + profile.t[5] = -(3*Power(a0,2)*aMax*jMax - 3*Power(af,2)*aMax*jMax - 6*a0*Power(aMax,2)*jMax + 6*af*Power(aMax,2)*jMax + 18*Power(aMax,3)*jMax - 6*Power(aMax,2)*Power(jMax,2)*tf - 6*aMax*Power(jMax,2)*v0 + 6*aMax*Power(jMax,2)*vf + Sqrt(3)*Sqrt(Power(aMax,2)*Power(jMax,2)*(-3*Power(a0,4) - 3*Power(af,4) + 4*Power(a0,3)*aMax - 4*Power(af,3)*aMax + 6*Power(a0,2)*(Power(af,2) + 2*af*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*(v0 - vf))) - 12*a0*aMax*(Power(af,2) + 2*af*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*(v0 - vf))) + 12*Power(af,2)*(Power(aMax,2) - aMax*jMax*tf + jMax*(-v0 + vf)) + 24*af*aMax*(Power(aMax,2) - aMax*jMax*tf + jMax*(-v0 + vf)) + 12*(Power(aMax,4) - 2*Power(aMax,3)*jMax*tf + Power(aMax,2)*Power(jMax,2)*Power(tf,2) - Power(jMax,2)*Power(v0 - vf,2) + 2*aMax*Power(jMax,2)*(2*p0 - 2*pf + tf*(v0 + vf))))))/(12.*Power(aMax,2)*Power(jMax,2)); + profile.t[6] = (af + aMax)/jMax; profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, -jMax, 0, jMax}); - if (profile.check(pf, vf, af, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, vMax, aMax)) { return true; } } @@ -31,15 +31,15 @@ bool RuckigStep2::time_up_acc0_acc1_vel(Profile& profile, double tf, double p0, // Profile UDUD { profile.t[0] = (-a0 + aMax)/jMax; - profile.t[1] = (3*Power(a0,4) - 4*Power(a0,3)*aMax + 24*a0*Power(aMax,3) - 6*Power(a0,2)*(Power(aMax,2) + 2*aMax*jMax*tf + 2*jMax*(v0 - vf)) - 12*(2*Power(aMax,4) - 2*Power(aMax,3)*jMax*tf - 2*aMax*Power(jMax,2)*(p0 - pf + tf*v0) - Power(jMax,2)*Power(v0 - vf,2) + Power(aMax,2)*jMax*(-v0 + vf)))/(12.*aMax*jMax*(Power(a0,2) - 2*a0*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*(-v0 + vf)))); + profile.t[1] = (3*Power(a0,4) + 3*Power(af,4) - 4*Power(a0,3)*aMax - 8*Power(af,3)*aMax + 24*a0*Power(aMax,3) + 24*af*aMax*(Power(aMax,2) + jMax*(v0 - vf)) - 6*Power(af,2)*(Power(aMax,2) + 2*jMax*(v0 - vf)) + 6*Power(a0,2)*(Power(af,2) - 2*af*aMax - Power(aMax,2) - 2*aMax*jMax*tf - 2*jMax*v0 + 2*jMax*vf) - 12*(2*Power(aMax,4) - 2*Power(aMax,3)*jMax*tf - 2*aMax*Power(jMax,2)*(p0 - pf + tf*v0) - Power(jMax,2)*Power(v0 - vf,2) + Power(aMax,2)*jMax*(-v0 + vf)))/(12.*aMax*jMax*(Power(a0,2) + Power(af,2) - 2*a0*aMax - 2*af*aMax + 2*(Power(aMax,2) - aMax*jMax*tf - jMax*v0 + jMax*vf))); profile.t[2] = aMax/jMax; - profile.t[3] = -(Power(a0,2) - 2*a0*aMax + 4*Power(aMax,2) - 2*aMax*jMax*tf - 2*jMax*v0 + 2*jMax*vf)/(2.*aMax*jMax); + profile.t[3] = -(Power(a0,2) + Power(af,2) - 2*a0*aMax - 2*af*aMax + 4*Power(aMax,2) - 2*aMax*jMax*tf - 2*jMax*v0 + 2*jMax*vf)/(2.*aMax*jMax); profile.t[4] = profile.t[2]; - profile.t[5] = (3*Power(a0,4) - 8*Power(a0,3)*aMax - 6*Power(a0,2)*(Power(aMax,2) + 2*jMax*(v0 - vf)) + 24*a0*(Power(aMax,3) + aMax*jMax*(v0 - vf)) - 12*(2*Power(aMax,4) - 2*Power(aMax,3)*jMax*tf - Power(jMax,2)*Power(v0 - vf,2) + Power(aMax,2)*jMax*(-v0 + vf) + 2*aMax*Power(jMax,2)*(p0 - pf + tf*vf)))/(12.*aMax*jMax*(Power(a0,2) - 2*a0*aMax + 2*(Power(aMax,2) - aMax*jMax*tf + jMax*(-v0 + vf)))); - profile.t[6] = profile.t[2]; + profile.t[5] = (3*Power(a0,4) + 3*Power(af,4) - 8*Power(a0,3)*aMax - 4*Power(af,3)*aMax + 24*af*Power(aMax,3) - 6*Power(af,2)*(Power(aMax,2) + 2*aMax*jMax*tf + 2*jMax*(v0 - vf)) + 6*Power(a0,2)*(Power(af,2) - Power(aMax,2) - 2*jMax*v0 + 2*jMax*vf) - 12*a0*aMax*(Power(af,2) - 2*(Power(aMax,2) + jMax*v0 - jMax*vf)) - 12*(2*Power(aMax,4) - 2*Power(aMax,3)*jMax*tf - Power(jMax,2)*Power(v0 - vf,2) + Power(aMax,2)*jMax*(-v0 + vf) + 2*aMax*Power(jMax,2)*(p0 - pf + tf*vf)))/(12.*aMax*jMax*(Power(a0,2) + Power(af,2) - 2*a0*aMax - 2*af*aMax + 2*(Power(aMax,2) - aMax*jMax*tf - jMax*v0 + jMax*vf))); + profile.t[6] = (-af + aMax)/jMax; profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, jMax, 0, -jMax}); - if (profile.check(pf, vf, af, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, vMax, aMax)) { return true; } } @@ -47,7 +47,7 @@ bool RuckigStep2::time_up_acc0_acc1_vel(Profile& profile, double tf, double p0, return false; } -bool RuckigStep2::time_up_acc1_vel(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { +bool RuckigStep2::time_up_acc1_vel(Profile& profile, double vMax, double aMax, double jMax) { // Profile UDDU { std::array polynom; @@ -68,7 +68,7 @@ bool RuckigStep2::time_up_acc1_vel(Profile& profile, double tf, double p0, doubl profile.t[6] = profile.t[4]; profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, -jMax, 0, jMax}); - if (profile.check(pf, vf, af, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, vMax, aMax)) { return true; } } @@ -94,7 +94,7 @@ bool RuckigStep2::time_up_acc1_vel(Profile& profile, double tf, double p0, doubl profile.t[6] = profile.t[4]; profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, jMax, 0, -jMax}); - if (profile.check(pf, vf, af, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, vMax, aMax)) { return true; } } @@ -103,7 +103,7 @@ bool RuckigStep2::time_up_acc1_vel(Profile& profile, double tf, double p0, doubl return false; } -bool RuckigStep2::time_up_acc0_vel(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { +bool RuckigStep2::time_up_acc0_vel(Profile& profile, double vMax, double aMax, double jMax) { // Profile UDDU { std::array polynom; @@ -124,7 +124,7 @@ bool RuckigStep2::time_up_acc0_vel(Profile& profile, double tf, double p0, doubl profile.t[6] = af/jMax + t; profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, -jMax, 0, jMax}); - if (profile.check(pf, vf, af, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, vMax, aMax)) { return true; } } @@ -150,7 +150,7 @@ bool RuckigStep2::time_up_acc0_vel(Profile& profile, double tf, double p0, doubl profile.t[6] = -(af/jMax) + t; profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, jMax, 0, -jMax}); - if (profile.check(pf, vf, af, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, vMax, aMax)) { return true; } } @@ -159,9 +159,11 @@ bool RuckigStep2::time_up_acc0_vel(Profile& profile, double tf, double p0, doubl return false; } -bool RuckigStep2::time_up_vel(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { +bool RuckigStep2::time_up_vel(Profile& profile, double vMax, double aMax, double jMax) { // Profile UDDU { + // std::cout << "HERE" << a0 << " " << tf << std::endl; + // Find root of 5th order polynom std::array polynom; polynom[0] = 1.0; @@ -171,8 +173,15 @@ bool RuckigStep2::time_up_vel(Profile& profile, double tf, double p0, double v0, 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)); + std::array deriv; + deriv[0] = 5 * polynom[0]; + deriv[1] = 4 * polynom[1]; + deriv[2] = 3 * polynom[2]; + deriv[3] = 2 * polynom[3]; + deriv[4] = polynom[4]; + // Solve 4th order derivative analytically - auto extremas = Roots::solveQuart(5 * polynom[0], 4 * polynom[1], 3 * polynom[2], 2 * polynom[3], polynom[4]); + auto extremas = Roots::solveQuart(deriv[0], deriv[1], deriv[2], deriv[3], deriv[4]); std::set> tz_intervals; double tz_min {0.0}; @@ -187,9 +196,16 @@ bool RuckigStep2::time_up_vel(Profile& profile, double tf, double p0, double v0, // Check that polynom(lower) and polynom(upper) have different signs (should only happen at first and last boundary) double val_current = Roots::polyEval(polynom, tz_current); double val_new = Roots::polyEval(polynom, tz); + // std::cout << "tz: " << tz << " " << val_new << " " << Roots::polyEval(deriv, tz) << std::endl; if (std::abs(val_new) < 1e-15) { - tz_intervals.insert({tz - 1e-12, tz + 1e-12}); - tz += 1e-14; + // if (val_current * val_new < 0) { + // tz_intervals.insert({tz_current, tz}); + // tz += 1e-14; + // } else { + tz_intervals.insert({tz - 1e-12, tz + 1e-12}); + tz += 1e-14; + // } + } else if (val_current * val_new < 0) { tz_intervals.insert({tz_current, tz}); } @@ -203,19 +219,27 @@ bool RuckigStep2::time_up_vel(Profile& profile, double tf, double p0, double v0, // Use safe Newton method double lower = std::get<0>(interval); double upper = std::get<1>(interval); - double tz = Roots::shrinkInterval(polynom, lower, upper, 1e-16); + double t = Roots::shrinkInterval(polynom, lower, upper, 2e-16); - double vPlat = Power(a0,2)/(2.*jMax) + 2*a0*tz + jMax*Power(tz,2) + v0; + // std::cout << std::setprecision(15) << t << " " << Roots::polyEval(polynom, t) << std::endl; + + double vPlat = Power(a0,2)/(2.*jMax) + 2*a0*t + jMax*Power(t,2) + v0; double h1 = 2*Sqrt((vPlat - vf)/jMax); - profile.t[0] = tz; + profile.t[0] = t; profile.t[1] = 0; - profile.t[2] = a0/jMax + tz; - profile.t[3] = -(4*Power(a0,3) + 12*a0*jMax*(v0 + jMax*tz*(3*tz + h1)) + 3*Power(a0,2)*jMax*(8*tz + h1) + 6*Power(jMax,2)*(2*p0 - 2*pf + 2*jMax*Power(tz,3) + 4*tz*v0 + jMax*Power(tz,2)*h1 + v0*h1 + h1*vf))/(6.*jMax*(Power(a0,2) + 4*a0*jMax*tz + 2*jMax*(jMax*Power(tz,2) + v0)));; + profile.t[2] = a0/jMax + t; + // profile.t[3] = -((23*Power(a0,7) + Power(a0,6)*jMax*(200*t + 39*tf) + 6*Power(a0,5)*jMax*(2*jMax*(45*Power(t,2) + 22*t*tf - 5*Power(tf,2)) + 13*(v0 - vf)) + 6*Power(a0,4)*Power(jMax,2)*(20*p0 - 20*pf + 2*jMax*t*(41*Power(t,2) + 52*t*tf - 23*Power(tf,2)) + 24*t*(v0 - vf) + 5*tf*(-3*v0 + 7*vf)) + 12*Power(a0,3)*Power(jMax,2)*(2*Power(jMax,2)*Power(t,2)*(6*Power(t,2) + 22*t*tf - 11*Power(tf,2)) - 19*Power(v0 - vf,2) + 2*jMax*(20*p0*t - 20*pf*t - 6*p0*tf + 6*pf*tf - 25*Power(t,2)*v0 - 26*t*tf*v0 + Power(tf,2)*v0 + 25*Power(t,2)*vf + 46*t*tf*vf - 7*Power(tf,2)*vf)) + 36*Power(a0,2)*Power(jMax,3)*(2*Power(jMax,2)*Power(t,3)*(2*t - tf)*tf - 2*jMax*(pf*(9*Power(t,2) + 2*t*tf - Power(tf,2)) + p0*(-9*Power(t,2) - 2*t*tf + Power(tf,2)) + 4*t*(3*t - tf)*(t + 2*tf)*v0 + (-12*Power(t,3) - 29*Power(t,2)*tf + 6*t*Power(tf,2) + Power(tf,3))*vf) - (v0 - vf)*(4*p0 - 4*pf + 40*t*(v0 - vf) + tf*(v0 + 3*vf))) + 72*a0*Power(jMax,3)*(-3*Power(v0 - vf,3) - 4*Power(jMax,2)*t*(pf*(Power(t,2) + 2*t*tf - Power(tf,2)) + p0*(-Power(t,2) - 2*t*tf + Power(tf,2)) + t*(Power(t,2) + 4*t*tf - 2*Power(tf,2))*v0 + (-Power(t,3) - 5*Power(t,2)*tf + Power(tf,3))*vf) + jMax*(Power(p0,2) + Power(pf,2) + 4*(-5*Power(t,2) + Power(tf,2))*Power(v0,2) + 2*(20*Power(t,2) - Power(tf,2))*v0*vf - (20*Power(t,2) + Power(tf,2))*Power(vf,2) + pf*tf*(-6*v0 + 4*vf) - 2*p0*(pf - 3*tf*v0 + 2*tf*vf))) + 72*Power(jMax,4)*(2*Power(jMax,2)*Power(t,2)*(2*t - tf)*tf*(p0 - pf - t*v0 + (t + tf)*vf) + Power(v0 - vf,2)*(4*p0 - 4*pf + tf*(v0 + 3*vf)) + jMax*(Power(p0,2)*(4*t - 3*tf) + Power(pf,2)*(4*t - 3*tf) - 2*pf*(Power(t,2) + 2*t*tf - Power(tf,2))*v0 + 2*t*(-3*Power(t,2) + Power(tf,2))*Power(v0,2) + 2*pf*(Power(t,2) - 2*t*tf + 2*Power(tf,2))*vf + 2*(6*Power(t,3) + Power(t,2)*tf - Power(tf,3))*v0*vf - (6*Power(t,3) + 2*Power(t,2)*tf - 2*t*Power(tf,2) + Power(tf,3))*Power(vf,2) - 2*p0*(pf*(4*t - 3*tf) + Power(t,2)*(-v0 + vf) - 2*t*tf*(v0 + vf) + Power(tf,2)*(v0 + 2*vf)))))/(jMax*(-Power(a0,6) + 6*Power(a0,4)*jMax*(v0 - vf) - 36*Power(a0,2)*Power(jMax,2)*Power(v0 - vf,2) + 48*Power(a0,3)*Power(jMax,2)*(p0 - pf + tf*vf) - 144*a0*Power(jMax,3)*(v0 - vf)*(p0 - pf + tf*vf) + 72*Power(jMax,3)*(Power(v0 - vf,3) + jMax*Power(p0 - pf + tf*vf,2))))); + // profile.t[4] = (12*Power(a0,7) + Power(a0,6)*jMax*(101*t + 19*tf) + 6*Power(a0,5)*jMax*(45*jMax*Power(t,2) + 22*jMax*t*tf - 5*jMax*Power(tf,2) + 6*v0 - 6*vf) + 6*Power(a0,4)*Power(jMax,2)*(6*p0 - 6*pf + jMax*t*(41*Power(t,2) + 52*t*tf - 23*Power(tf,2)) - 7*tf*v0 + 11*t*(v0 - vf) + 13*tf*vf) + 12*Power(a0,3)*Power(jMax,2)*(Power(jMax,2)*Power(t,2)*(6*Power(t,2) + 22*t*tf - 11*Power(tf,2)) - 8*Power(v0 - vf,2) + jMax*(16*p0*t - 16*pf*t - 4*p0*tf + 4*pf*tf - 25*Power(t,2)*v0 - 26*t*tf*v0 + Power(tf,2)*v0 + 25*Power(t,2)*vf + 42*t*tf*vf - 5*Power(tf,2)*vf)) + 144*a0*Power(jMax,3)*(-Power(v0 - vf,3) + jMax*(v0 - vf)*(p0*(t + tf) - pf*(t + tf) - 5*Power(t,2)*v0 + Power(tf,2)*v0 + 5*Power(t,2)*vf + t*tf*vf) + Power(jMax,2)*t*(p0*(Power(t,2) + 2*t*tf - Power(tf,2)) + pf*(-Power(t,2) - 2*t*tf + Power(tf,2)) - t*(Power(t,2) + 4*t*tf - 2*Power(tf,2))*v0 + (Power(t,3) + 5*Power(t,2)*tf - Power(tf,3))*vf)) + 36*Power(a0,2)*Power(jMax,3)*(Power(jMax,2)*Power(t,3)*(2*t - tf)*tf - (19*t + tf)*Power(v0 - vf,2) - jMax*(pf*(9*Power(t,2) + 2*t*tf - Power(tf,2)) + p0*(-9*Power(t,2) - 2*t*tf + Power(tf,2)) + 4*t*(3*t - tf)*(t + 2*tf)*v0 + (-12*Power(t,3) - 29*Power(t,2)*tf + 6*t*Power(tf,2) + Power(tf,3))*vf)) + 72*Power(jMax,4)*(Power(jMax,2)*Power(t,2)*(2*t - tf)*tf*(p0 - pf - t*v0 + (t + tf)*vf) + Power(v0 - vf,2)*(2*p0 - 2*pf - t*v0 + tf*v0 + (t + tf)*vf) + jMax*(Power(p0,2)*(t - tf) + Power(pf,2)*(t - tf) + pf*(-Power(t,2) - 2*t*tf + Power(tf,2))*v0 + t*(-3*Power(t,2) + Power(tf,2))*Power(v0,2) + pf*(Power(t,2) + Power(tf,2))*vf + (6*Power(t,3) + Power(t,2)*tf - Power(tf,3))*v0*vf - Power(t,2)*(3*t + tf)*Power(vf,2) - p0*(2*pf*(t - tf) - 2*t*tf*v0 + Power(t,2)*(-v0 + vf) + Power(tf,2)*(v0 + vf)))))/(jMax*(-Power(a0,6) + 6*Power(a0,4)*jMax*(v0 - vf) - 36*Power(a0,2)*Power(jMax,2)*Power(v0 - vf,2) + 48*Power(a0,3)*Power(jMax,2)*(p0 - pf + tf*vf) - 144*a0*Power(jMax,3)*(v0 - vf)*(p0 - pf + tf*vf) + 72*Power(jMax,3)*(Power(v0 - vf,3) + jMax*Power(p0 - pf + tf*vf,2)))); + // profile.t[5] = 0.0; + // profile.t[6] = (12*Power(a0,7) + Power(a0,6)*jMax*(101*t + 19*tf) + 6*Power(a0,5)*jMax*(45*jMax*Power(t,2) + 22*jMax*t*tf - 5*jMax*Power(tf,2) + 6*v0 - 6*vf) + 6*Power(a0,4)*Power(jMax,2)*(6*p0 - 6*pf + jMax*t*(41*Power(t,2) + 52*t*tf - 23*Power(tf,2)) - 7*tf*v0 + 11*t*(v0 - vf) + 13*tf*vf) + 12*Power(a0,3)*Power(jMax,2)*(Power(jMax,2)*Power(t,2)*(6*Power(t,2) + 22*t*tf - 11*Power(tf,2)) - 8*Power(v0 - vf,2) + jMax*(16*p0*t - 16*pf*t - 4*p0*tf + 4*pf*tf - 25*Power(t,2)*v0 - 26*t*tf*v0 + Power(tf,2)*v0 + 25*Power(t,2)*vf + 42*t*tf*vf - 5*Power(tf,2)*vf)) + 144*a0*Power(jMax,3)*(-Power(v0 - vf,3) + jMax*(v0 - vf)*(p0*(t + tf) - pf*(t + tf) - 5*Power(t,2)*v0 + Power(tf,2)*v0 + 5*Power(t,2)*vf + t*tf*vf) + Power(jMax,2)*t*(p0*(Power(t,2) + 2*t*tf - Power(tf,2)) + pf*(-Power(t,2) - 2*t*tf + Power(tf,2)) - t*(Power(t,2) + 4*t*tf - 2*Power(tf,2))*v0 + (Power(t,3) + 5*Power(t,2)*tf - Power(tf,3))*vf)) + 36*Power(a0,2)*Power(jMax,3)*(Power(jMax,2)*Power(t,3)*(2*t - tf)*tf - (19*t + tf)*Power(v0 - vf,2) - jMax*(pf*(9*Power(t,2) + 2*t*tf - Power(tf,2)) + p0*(-9*Power(t,2) - 2*t*tf + Power(tf,2)) + 4*t*(3*t - tf)*(t + 2*tf)*v0 + (-12*Power(t,3) - 29*Power(t,2)*tf + 6*t*Power(tf,2) + Power(tf,3))*vf)) + 72*Power(jMax,4)*(Power(jMax,2)*Power(t,2)*(2*t - tf)*tf*(p0 - pf - t*v0 + (t + tf)*vf) + Power(v0 - vf,2)*(2*p0 - 2*pf - t*v0 + tf*v0 + (t + tf)*vf) + jMax*(Power(p0,2)*(t - tf) + Power(pf,2)*(t - tf) + pf*(-Power(t,2) - 2*t*tf + Power(tf,2))*v0 + t*(-3*Power(t,2) + Power(tf,2))*Power(v0,2) + pf*(Power(t,2) + Power(tf,2))*vf + (6*Power(t,3) + Power(t,2)*tf - Power(tf,3))*v0*vf - Power(t,2)*(3*t + tf)*Power(vf,2) - p0*(2*pf*(t - tf) - 2*t*tf*v0 + Power(t,2)*(-v0 + vf) + Power(tf,2)*(v0 + vf)))))/(jMax*(-Power(a0,6) + 6*Power(a0,4)*jMax*(v0 - vf) - 36*Power(a0,2)*Power(jMax,2)*Power(v0 - vf,2) + 48*Power(a0,3)*Power(jMax,2)*(p0 - pf + tf*vf) - 144*a0*Power(jMax,3)*(v0 - vf)*(p0 - pf + tf*vf) + 72*Power(jMax,3)*(Power(v0 - vf,3) + jMax*Power(p0 - pf + tf*vf,2)))); + + profile.t[3] = -(4*Power(a0,3) + 12*a0*jMax*(v0 + jMax*t*(3*t + h1)) + 3*Power(a0,2)*jMax*(8*t + h1) + 6*Power(jMax,2)*(2*p0 - 2*pf + 2*jMax*Power(t,3) + 4*t*v0 + jMax*Power(t,2)*h1 + v0*h1 + h1*vf))/(6.*jMax*(Power(a0,2) + 4*a0*jMax*t + 2*jMax*(jMax*Power(t,2) + v0))); profile.t[4] = Sqrt((-vf + vPlat)/jMax); profile.t[5] = 0; profile.t[6] = profile.t[4]; + // std::cout << profile.t[0] + profile.t[2] + profile.t[3] + profile.t[4] + profile.t[6] << " " << tf << " " << (6.*jMax*(Power(a0,2) + 4*a0*jMax*t + 2*jMax*(jMax*Power(t,2) + v0))) << std::endl; // std::cout << profile.t[0] << std::endl; // std::cout << profile.t[1] << std::endl; // std::cout << profile.t[2] << std::endl; @@ -226,7 +250,7 @@ bool RuckigStep2::time_up_vel(Profile& profile, double tf, double p0, double v0, // std::cout << "---" << std::endl; profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, -jMax, 0, jMax}); - if (profile.check(pf, vf, af, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, vMax, aMax)) { return true; } } @@ -312,10 +336,8 @@ bool RuckigStep2::time_up_vel(Profile& profile, double tf, double p0, double v0, profile.t[5] = 0; profile.t[6] = profile.t[4]; - // std::cout << profile.t[4] << " " << Sqrt((vf - vPlat)/jMax) << std::endl; - profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, jMax, 0, -jMax}); - if (profile.check(pf, vf, af, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, vMax, aMax)) { return true; } } @@ -324,8 +346,8 @@ bool RuckigStep2::time_up_vel(Profile& profile, double tf, double p0, double v0, return false; } -bool RuckigStep2::time_up_acc0_acc1(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { - if (std::abs(a0) < 1e-16) { +bool RuckigStep2::time_up_acc0_acc1(Profile& profile, double vMax, double aMax, double jMax) { + if (std::abs(a0) < DBL_EPSILON) { profile.t[0] = (Power(aMax,2)*Power(tf,2) - Power(v0 - vf,2) + 2*aMax*(2*p0 - 2*pf + tf*(v0 + vf)))/(2.*Power(aMax,2)*tf); profile.t[1] = -(Power(aMax,2)*Power(tf,2) - 2*Power(v0 - vf,2) + aMax*(8*p0 - 8*pf + 5*tf*v0 + 3*tf*vf))/(2.*Power(aMax,2)*tf); profile.t[2] = profile.t[0]; @@ -335,8 +357,11 @@ bool RuckigStep2::time_up_acc0_acc1(Profile& profile, double tf, double p0, doub profile.t[6] = profile.t[0]; jMax = aMax/profile.t[0]; + // std::cout << profile.t[0] << std::endl; + // std::cout << jMax << std::endl; + profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, -jMax, 0, jMax}); - return profile.check(pf, vf, af, vMax, aMax); + return profile.check(tf, pf, vf, af, vMax, aMax); } double h1 = -12*(2*Power(aMax,3)*tf + Power(a0,2)*(aMax*tf - v0 + vf) - 2*a0*aMax*(aMax*tf - v0 + vf)); @@ -357,10 +382,10 @@ bool RuckigStep2::time_up_acc0_acc1(Profile& profile, double tf, double p0, doub jMax = h4/(2*h2); profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, -jMax, 0, jMax}); - return profile.check(pf, vf, af, vMax, aMax); + return profile.check(tf, pf, vf, af, vMax, aMax); } -bool RuckigStep2::time_up_acc1(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { +bool RuckigStep2::time_up_acc1(Profile& profile, double vMax, double aMax, double jMax) { // a3 != 0 // Case UDDU, Solution 2 @@ -374,7 +399,7 @@ bool RuckigStep2::time_up_acc1(Profile& profile, double tf, double p0, double v0 profile.t[6] = (aMax/jMax); profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, -jMax, 0, jMax}); - if (profile.check(pf, vf, af, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, vMax, aMax)) { return true; } } @@ -390,14 +415,14 @@ bool RuckigStep2::time_up_acc1(Profile& profile, double tf, double p0, double v0 profile.t[6] = (aMax/jMax); profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, jMax, 0, -jMax}); - if (profile.check(pf, vf, af, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, vMax, aMax)) { return true; } } return false; } -bool RuckigStep2::time_up_acc0(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { +bool RuckigStep2::time_up_acc0(Profile& profile, double vMax, double aMax, double jMax) { // a3 != 0 // Solution 1 @@ -410,15 +435,15 @@ bool RuckigStep2::time_up_acc0(Profile& profile, double tf, double p0, double v0 profile.t[6] = 0; profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, -jMax, 0, jMax}); - if (profile.check(pf, vf, af, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, vMax, aMax)) { return true; } return false; } -bool RuckigStep2::time_up_none(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { - if (std::abs(v0) < DBL_EPSILON && std::abs(a0) < DBL_EPSILON && std::abs(vf) < DBL_EPSILON) { +bool RuckigStep2::time_up_none(Profile& profile, double vMax, double aMax, double jMax) { + if (std::abs(v0) < DBL_EPSILON && std::abs(a0) < DBL_EPSILON && std::abs(vf) < DBL_EPSILON && std::abs(af) < DBL_EPSILON) { profile.t[0] = tf/4; profile.t[1] = 0; profile.t[2] = profile.t[0]; @@ -430,7 +455,7 @@ bool RuckigStep2::time_up_none(Profile& profile, double tf, double p0, double v0 double jMaxNew = (-32*(p0 - pf))/Power(tf,3); profile.set(p0, v0, a0, {jMaxNew, 0, -jMaxNew, 0, -jMaxNew, 0, jMaxNew}); - return profile.check(pf, vf, af, vMax, aMax); + return profile.check(tf, pf, vf, af, vMax, aMax); } if (std::abs(v0) < DBL_EPSILON && std::abs(a0) < DBL_EPSILON) { @@ -446,7 +471,7 @@ bool RuckigStep2::time_up_none(Profile& profile, double tf, double p0, double v0 double jMaxNew = (4*(-4*p0*tf + 4*pf*tf - 2*Power(tf,2)*vf + h1))/Power(tf,4); profile.set(p0, v0, a0, {jMaxNew, 0, -jMaxNew, 0, -jMaxNew, 0, jMaxNew}); - return profile.check(pf, vf, af, vMax, aMax); + return profile.check(tf, pf, vf, af, vMax, aMax); } if (std::abs(a0) < DBL_EPSILON && std::abs(vf) < DBL_EPSILON) { @@ -463,7 +488,7 @@ bool RuckigStep2::time_up_none(Profile& profile, double tf, double p0, double v0 double jMaxNew = (-4*(4*p0*tf - 4*pf*tf + 2*Power(tf,2)*v0 + Sqrt(Power(tf,2)*(Power(tf,2)*Power(v0,2) + 4*Power(2*p0 - 2*pf + tf*v0,2)))))/Power(tf,4); profile.set(p0, v0, a0, {jMaxNew, 0, -jMaxNew, 0, -jMaxNew, 0, jMaxNew}); - if (profile.check(pf, vf, af, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, vMax, aMax)) { return true; } } @@ -503,7 +528,7 @@ bool RuckigStep2::time_up_none(Profile& profile, double tf, double p0, double v0 // std::cout << "---" << std::endl; profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, -jMax, 0, jMax}); - if (profile.check(pf, vf, af, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, vMax, aMax)) { return true; } } @@ -533,7 +558,7 @@ bool RuckigStep2::time_up_none(Profile& profile, double tf, double p0, double v0 profile.t[6] = (Power(a0,3)*(-t + tf) + 3*Power(a0,2)*jMax*(Power(t,2) - t*tf + Power(tf,2)) + 3*a0*jMax*(-6*p0 + 6*pf - tf*(jMax*(-2*Power(t,2) + t*tf + Power(tf,2)) - 2*v0 + 8*vf)) - 3*jMax*(Power(jMax,2)*t*(t - tf)*Power(tf,2) - 4*Power(v0 - vf,2) + jMax*(-8*p0*t + 8*pf*t + 2*p0*tf - 2*pf*tf - 4*Power(t,2)*v0 + 2*Power(tf,2)*v0 + 4*Power(t,2)*vf - 8*t*tf*vf)))/(Power(a0,3) + 3*Power(a0,2)*jMax*tf + 6*a0*jMax*(v0 - vf) + 6*Power(jMax,2)*(2*p0 - 2*pf + tf*(v0 + vf))); profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, -jMax, 0, jMax}); - if (profile.check(pf, vf, af, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, vMax, aMax)) { return true; } } @@ -574,7 +599,7 @@ bool RuckigStep2::time_up_none(Profile& profile, double tf, double p0, double v0 // std::cout << "---" << std::endl; profile.set(p0, v0, a0, {jMax, 0, -jMax, 0, jMax, 0, -jMax}); - if (profile.check(pf, vf, af, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, vMax, aMax)) { return true; } } @@ -584,142 +609,142 @@ bool RuckigStep2::time_up_none(Profile& profile, double tf, double p0, double v0 return false; } -bool RuckigStep2::time_down_acc0_acc1_vel(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { - return time_up_acc0_acc1_vel(profile, tf, p0, v0, a0, pf, vf, af, -vMax, -aMax, -jMax); +bool RuckigStep2::time_down_acc0_acc1_vel(Profile& profile, double vMax, double aMax, double jMax) { + return time_up_acc0_acc1_vel(profile, -vMax, -aMax, -jMax); } -bool RuckigStep2::time_down_acc1_vel(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { - return time_up_acc1_vel(profile, tf, p0, v0, a0, pf, vf, af, -vMax, -aMax, -jMax); +bool RuckigStep2::time_down_acc1_vel(Profile& profile, double vMax, double aMax, double jMax) { + return time_up_acc1_vel(profile, -vMax, -aMax, -jMax); } -bool RuckigStep2::time_down_acc0_vel(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { - return time_up_acc0_vel(profile, tf, p0, v0, a0, pf, vf, af, -vMax, -aMax, -jMax); +bool RuckigStep2::time_down_acc0_vel(Profile& profile, double vMax, double aMax, double jMax) { + return time_up_acc0_vel(profile, -vMax, -aMax, -jMax); } -bool RuckigStep2::time_down_vel(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { - return time_up_vel(profile, tf, p0, v0, a0, pf, vf, af, -vMax, -aMax, -jMax); +bool RuckigStep2::time_down_vel(Profile& profile, double vMax, double aMax, double jMax) { + return time_up_vel(profile, -vMax, -aMax, -jMax); } -bool RuckigStep2::time_down_acc0_acc1(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { - return time_up_acc0_acc1(profile, tf, p0, v0, a0, pf, vf, af, -vMax, -aMax, -jMax); +bool RuckigStep2::time_down_acc0_acc1(Profile& profile, double vMax, double aMax, double jMax) { + return time_up_acc0_acc1(profile, -vMax, -aMax, -jMax); } -bool RuckigStep2::time_down_acc1(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { - return time_up_acc1(profile, tf, p0, v0, a0, pf, vf, af, -vMax, -aMax, -jMax); +bool RuckigStep2::time_down_acc1(Profile& profile, double vMax, double aMax, double jMax) { + return time_up_acc1(profile, -vMax, -aMax, -jMax); } -bool RuckigStep2::time_down_acc0(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { - return time_up_acc0(profile, tf, p0, v0, a0, pf, vf, af, -vMax, -aMax, -jMax); +bool RuckigStep2::time_down_acc0(Profile& profile, double vMax, double aMax, double jMax) { + return time_up_acc0(profile, -vMax, -aMax, -jMax); } -bool RuckigStep2::time_down_none(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { - return time_up_none(profile, tf, p0, v0, a0, pf, vf, af, -vMax, -aMax, -jMax); +bool RuckigStep2::time_down_none(Profile& profile, double vMax, double aMax, double jMax) { + return time_up_none(profile, -vMax, -aMax, -jMax); } -bool RuckigStep2::get_profile(Profile& profile, double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double aMax, double jMax) { +bool RuckigStep2::get_profile(Profile& profile, double vMax, double aMax, double jMax) { // Test all cases to get ones that match if (pf > p0) { - if (time_up_none(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { - profile.type = Profile::Type::UP_NONE; - - } else if (time_down_none(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { - profile.type = Profile::Type::DOWN_NONE; - - } else if (time_up_vel(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { - profile.type = Profile::Type::UP_VEL; - - } else if (time_down_vel(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { - profile.type = Profile::Type::DOWN_VEL; - - } else if (time_up_acc0_acc1_vel(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + if (time_up_acc0_acc1_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC0_ACC1_VEL; - } else if (time_down_acc0_acc1_vel(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_acc0_acc1_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC0_ACC1_VEL; - } else if (time_up_acc0_vel(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_up_acc0_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC0_VEL; - } else if (time_down_acc0_vel(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_acc0_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC0_VEL; - } else if (time_up_acc1_vel(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_up_acc1_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC1_VEL; - } else if (time_down_acc1_vel(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_acc1_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC1_VEL; - } else if (time_up_acc0_acc1(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { - profile.type = Profile::Type::UP_ACC0_ACC1; + } else if (time_up_vel(profile, vMax, aMax, jMax)) { + profile.type = Profile::Type::UP_VEL; - } else if (time_down_acc0_acc1(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { - profile.type = Profile::Type::DOWN_ACC0_ACC1; + } else if (time_down_vel(profile, vMax, aMax, jMax)) { + profile.type = Profile::Type::DOWN_VEL; - } else if (time_up_acc1(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } 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; + + } else if (time_up_acc1(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC1; - } else if (time_down_acc1(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { - profile.type = Profile::Type::DOWN_ACC1; + } else if (time_up_acc0_acc1(profile, vMax, aMax, jMax)) { + profile.type = Profile::Type::UP_ACC0_ACC1; - } else if (time_up_acc0(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { - profile.type = Profile::Type::UP_ACC0; + } else if (time_down_none(profile, vMax, aMax, jMax)) { + profile.type = Profile::Type::DOWN_NONE; - } else if (time_down_acc0(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_acc0(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC0; + } else if (time_down_acc1(profile, vMax, aMax, jMax)) { + profile.type = Profile::Type::DOWN_ACC1; + + } else if (time_down_acc0_acc1(profile, vMax, aMax, jMax)) { + profile.type = Profile::Type::DOWN_ACC0_ACC1; + } else { return false; } } else { - if (time_down_none(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { - profile.type = Profile::Type::DOWN_NONE; - - } else if (time_up_none(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { - profile.type = Profile::Type::UP_NONE; - - } else if (time_down_vel(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { - profile.type = Profile::Type::DOWN_VEL; - - } else if (time_up_vel(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { - profile.type = Profile::Type::UP_VEL; - - } else if (time_down_acc0_acc1_vel(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + if (time_down_acc0_acc1_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC0_ACC1_VEL; - } else if (time_up_acc0_acc1_vel(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_up_acc0_acc1_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC0_ACC1_VEL; - } else if (time_down_acc0_vel(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_acc0_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC0_VEL; - } else if (time_up_acc0_vel(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_up_acc0_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC0_VEL; - } else if (time_down_acc1_vel(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_acc1_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC1_VEL; - } else if (time_up_acc1_vel(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_up_acc1_vel(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC1_VEL; - } else if (time_down_acc0_acc1(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { - profile.type = Profile::Type::DOWN_ACC0_ACC1; + } else if (time_down_vel(profile, vMax, aMax, jMax)) { + profile.type = Profile::Type::DOWN_VEL; - } else if (time_up_acc0_acc1(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { - profile.type = Profile::Type::UP_ACC0_ACC1; + } else if (time_up_vel(profile, vMax, aMax, jMax)) { + profile.type = Profile::Type::UP_VEL; + + } else if (time_down_none(profile, vMax, aMax, jMax)) { + profile.type = Profile::Type::DOWN_NONE; - } else if (time_down_acc1(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_down_acc0(profile, vMax, aMax, jMax)) { + profile.type = Profile::Type::DOWN_ACC0; + + } else if (time_down_acc1(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::DOWN_ACC1; - } else if (time_up_acc1(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { - profile.type = Profile::Type::UP_ACC1; + } else if (time_down_acc0_acc1(profile, vMax, aMax, jMax)) { + profile.type = Profile::Type::DOWN_ACC0_ACC1; - } else if (time_down_acc0(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { - profile.type = Profile::Type::DOWN_ACC0; + } else if (time_up_none(profile, vMax, aMax, jMax)) { + profile.type = Profile::Type::UP_NONE; - } else if (time_up_acc0(profile, tf, p0, v0, a0, pf, vf, af, vMax, aMax, jMax)) { + } else if (time_up_acc0(profile, vMax, aMax, jMax)) { profile.type = Profile::Type::UP_ACC0; + } else if (time_up_acc1(profile, vMax, aMax, jMax)) { + profile.type = Profile::Type::UP_ACC1; + + } else if (time_up_acc0_acc1(profile, vMax, aMax, jMax)) { + profile.type = Profile::Type::UP_ACC0_ACC1; + } else { return false; } diff --git a/test/otg-test.cpp b/test/otg-test.cpp index 6f24baf9..e5a1fbd0 100644 --- a/test/otg-test.cpp +++ b/test/otg-test.cpp @@ -45,12 +45,10 @@ void check_calculation(OTGType& otg, InputParameter& input) { auto result = otg.update(input, output); CHECK( result == Result::Working ); - CHECK_FALSE( std::isnan(output.duration) ); + CHECK( output.duration > 0.0 ); for (size_t dof = 0; dof < DOFs; dof += 1) { - CHECK_FALSE( std::isnan(output.new_position[dof]) ); - CHECK_FALSE( std::isnan(output.new_velocity[dof]) ); - CHECK_FALSE( std::isnan(output.new_acceleration[dof]) ); + CHECK_FALSE( (std::isnan(output.new_position[dof]) || std::isnan(output.new_velocity[dof]) || std::isnan(output.new_acceleration[dof])) ); } } @@ -76,15 +74,7 @@ void check_comparison(OTGType& otg, InputParameter& input, OTGCompType& ot otg_comparison.atTime(half_duration, output_comparison); for (size_t dof = 0; dof < DOFs; dof += 1) { - CHECK_FALSE( std::isnan(output.new_position[dof]) ); - CHECK_FALSE( std::isnan(output.new_velocity[dof]) ); - CHECK_FALSE( std::isnan(output.new_acceleration[dof]) ); - - if constexpr (DOFs <= 2) { - CHECK( output.new_position[dof] == Approx(output_comparison.new_position[dof]).margin(1e-7) ); - // CHECK( output.new_velocity[dof] == Approx(output_comparison.new_velocity[dof]).margin(1e-7) ); - // CHECK( output.new_acceleration[dof] == Approx(output_comparison.new_acceleration[dof]).margin(1e-7) ); - } + CHECK( output.new_position[dof] == Approx(output_comparison.new_position[dof]).margin(1e-7) ); } } else { WARN("Ruckig and Reflexxes differ! Maybe Reflexxes error..."); @@ -195,12 +185,12 @@ TEST_CASE("Ruckig") { std::default_random_engine gen; std::uniform_real_distribution dist(0.0, 1.0); - for (size_t i = 0; i < 32*1024; i += 1) { + for (size_t i = 0; i < 64*1024; i += 1) { input.current_position = Vec::Random(); input.current_velocity = dist(gen) < 0.9 ? (Vec)Vec::Random() : (Vec)Vec::Zero(); input.current_acceleration = dist(gen) < 0.8 ? (Vec)Vec::Random() : (Vec)Vec::Zero(); input.target_position = Vec::Random(); - input.max_velocity = 10 * Vec::Random().array().abs() + 0.1; + input.max_velocity = 10 * Vec::Random().array().abs() + input.target_velocity.array().abs() + 0.1; input.max_acceleration = 10 * Vec::Random().array().abs() + 0.1; input.max_jerk = 10 * Vec::Random().array().abs() + 0.1; @@ -218,24 +208,12 @@ TEST_CASE("Ruckig") { std::default_random_engine gen; std::uniform_real_distribution dist(0.0, 1.0); - for (size_t i = 0; i < 32*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.85 ? (Vec1)Vec1::Random() : (Vec1)Vec1::Zero(); - input.target_position = Vec1::Random(); - input.max_velocity = 10 * Vec1::Random().array().abs() + 0.1; - input.max_acceleration = 10 * Vec1::Random().array().abs() + 0.1; - input.max_jerk = 10 * Vec1::Random().array().abs() + 0.1; - - check_comparison(otg, input, rflx); - } - - for (size_t i = 0; i < 32*1024; i += 1) { + for (size_t i = 0; i < 64*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 = Vec1::Random(); + input.target_velocity = dist(gen) < 0.6 ? (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() + 0.1; input.max_jerk = 10 * Vec1::Random().array().abs() + 0.1; @@ -253,7 +231,7 @@ TEST_CASE("Ruckig") { std::default_random_engine gen; std::uniform_real_distribution dist(0.0, 1.0); - for (size_t i = 0; i < 0; i += 1) { + for (size_t i = 0; i < 96; 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 fc833475..070200ea 100644 --- a/test/otg_plot.py +++ b/test/otg_plot.py @@ -95,15 +95,15 @@ def print_input_for_mathematica(inp, dof, tf=None): if __name__ == '__main__': inp = InputParameter() - inp.current_position = [0.393732] - inp.current_velocity = [-0.545762] - inp.current_acceleration = [-0.618779] - inp.target_position = [0.185249] - inp.target_velocity = [-0.515307] + 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 = [8.22285] - inp.max_acceleration = [0.774097] - inp.max_jerk = [9.64989] + inp.max_velocity = [1.073347279] + inp.max_acceleration = [0.3522756207] + inp.max_jerk = [0.103642817] inp.minimum_duration = None # print_input_for_mathematica(inp, 0)