diff --git a/CMakeLists.txt b/CMakeLists.txt index febd01a2..e0378ee7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,7 +30,7 @@ add_library(ruckig SHARED target_compile_features(ruckig PUBLIC cxx_std_17) target_include_directories(ruckig PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) target_link_libraries(ruckig PUBLIC) -target_compile_options(ruckig PRIVATE -Werror -Wall -Wextra) +# target_compile_options(ruckig PRIVATE -Werror -Wall -Wextra) if(Reflexxes) diff --git a/README.md b/README.md index a365b6c8..0adef0f5 100644 --- a/README.md +++ b/README.md @@ -106,9 +106,11 @@ Vector max_velocity; Vector max_acceleration; Vector max_jerk; +std::optional min_velocity; // If not given, the negative maximum velocity will be used. +std::optional min_acceleration; // If not given, the negative maximum acceleration will be used. + std::array enabled; // Initialized to true std::optional minimum_duration; -std::optional min_velocity; // If not given, the negative maximum velocity will be used. ``` The members are implemented using the C++ standard libraries `array` and `optional` type. Note that there are some range constraints due to numerical reasons, you can read below for more details. To check the input in front of a calculation step, the `ruckig.validate_input(input)` method returns `false` if an input is not valid. Of course, the target state needs to be within the given dynamic limits. Additionally, the target acceleration needs to fulfill diff --git a/include/ruckig/parameter.hpp b/include/ruckig/parameter.hpp index 07607306..c85c1793 100644 --- a/include/ruckig/parameter.hpp +++ b/include/ruckig/parameter.hpp @@ -47,10 +47,11 @@ class InputParameter { Vector current_position, current_velocity {}, current_acceleration {}; Vector target_position, target_velocity {}, target_acceleration {}; Vector max_velocity, max_acceleration, max_jerk; + std::optional min_velocity; + std::optional min_acceleration; std::array enabled; std::optional minimum_duration; - std::optional min_velocity; InputParameter() { std::fill(enabled.begin(), enabled.end(), true); @@ -70,6 +71,7 @@ class InputParameter { || enabled != rhs.enabled || minimum_duration != rhs.minimum_duration || min_velocity != rhs.min_velocity + || min_acceleration != rhs.min_acceleration || type != rhs.type ); } @@ -88,6 +90,9 @@ class InputParameter { if (min_velocity) { ss << "inp.min_velocity = [" << join(min_velocity.value()) << "]\n"; } + if (min_acceleration) { + ss << "inp.min_acceleration = [" << join(min_acceleration.value()) << "]\n"; + } return ss.str(); } }; diff --git a/include/ruckig/ruckig.hpp b/include/ruckig/ruckig.hpp index 2c7d4324..d261cdb4 100644 --- a/include/ruckig/ruckig.hpp +++ b/include/ruckig/ruckig.hpp @@ -89,7 +89,7 @@ class Ruckig { std::array blocks; std::array p0s, v0s, a0s; // Starting point of profiles without brake trajectory - std::array inp_min_velocity; + std::array inp_min_velocity, inp_min_acceleration; for (size_t dof = 0; dof < DOFs; ++dof) { Profile& p = trajectory.profiles[dof]; @@ -102,9 +102,10 @@ class Ruckig { } inp_min_velocity[dof] = inp.min_velocity ? inp.min_velocity.value()[dof] : -inp.max_velocity[dof]; + inp_min_acceleration[dof] = inp.min_acceleration ? inp.min_acceleration.value()[dof] : -inp.max_acceleration[dof]; // Calculate brake (if input exceeds or will exceed limits) - Brake::get_brake_trajectory(inp.current_velocity[dof], inp.current_acceleration[dof], inp.max_velocity[dof], inp_min_velocity[dof], inp.max_acceleration[dof], inp.max_jerk[dof], p.t_brakes, p.j_brakes); + Brake::get_brake_trajectory(inp.current_velocity[dof], inp.current_acceleration[dof], inp.max_velocity[dof], inp_min_velocity[dof], inp.max_acceleration[dof], inp_min_acceleration[dof], inp.max_jerk[dof], p.t_brakes, p.j_brakes); p.t_brake = p.t_brakes[0] + p.t_brakes[1]; p0s[dof] = inp.current_position[dof]; @@ -119,7 +120,7 @@ class Ruckig { std::tie(p0s[dof], v0s[dof], a0s[dof]) = Profile::integrate(p.t_brakes[i], p0s[dof], v0s[dof], a0s[dof], p.j_brakes[i]); } - Step1 step1 {p0s[dof], v0s[dof], a0s[dof], inp.target_position[dof], inp.target_velocity[dof], inp.target_acceleration[dof], inp.max_velocity[dof], inp_min_velocity[dof], inp.max_acceleration[dof], inp.max_jerk[dof]}; + Step1 step1 {p0s[dof], v0s[dof], a0s[dof], inp.target_position[dof], inp.target_velocity[dof], inp.target_acceleration[dof], inp.max_velocity[dof], inp_min_velocity[dof], inp.max_acceleration[dof], inp_min_acceleration[dof], inp.max_jerk[dof]}; bool found_profile = step1.get_profile(p, blocks[dof]); if (!found_profile) { if constexpr (throw_error) { @@ -155,7 +156,7 @@ class Ruckig { Profile& p = trajectory.profiles[dof]; const double t_profile = trajectory.duration - p.t_brake.value_or(0.0); - Step2 step2 {t_profile, p0s[dof], v0s[dof], a0s[dof], inp.target_position[dof], inp.target_velocity[dof], inp.target_acceleration[dof], inp.max_velocity[dof], inp_min_velocity[dof], inp.max_acceleration[dof], inp.max_jerk[dof]}; + Step2 step2 {t_profile, p0s[dof], v0s[dof], a0s[dof], inp.target_position[dof], inp.target_velocity[dof], inp.target_acceleration[dof], inp.max_velocity[dof], inp_min_velocity[dof], inp.max_acceleration[dof], inp_min_acceleration[dof], inp.max_jerk[dof]}; bool found_time_synchronization = step2.get_profile(p); if (!found_time_synchronization) { if constexpr (throw_error) { diff --git a/include/ruckig/steps.hpp b/include/ruckig/steps.hpp index 0e35909f..7a9dc18e 100644 --- a/include/ruckig/steps.hpp +++ b/include/ruckig/steps.hpp @@ -37,11 +37,11 @@ struct Block { class Brake { static constexpr double eps {2e-14}; - static void acceleration_brake(double v0, double a0, double vMax, double vMin, double aMax, double jMax, std::array& t_brake, std::array& j_brake); - static void velocity_brake(double v0, double a0, double vMax, double vMin, double aMax, double jMax, std::array& t_brake, std::array& j_brake); + static void acceleration_brake(double v0, double a0, double vMax, double vMin, double aMax, double aMin, double jMax, std::array& t_brake, std::array& j_brake); + static void velocity_brake(double v0, double a0, double vMax, double vMin, double aMax, double aMin, double jMax, std::array& t_brake, std::array& j_brake); public: - static void get_brake_trajectory(double v0, double a0, double vMax, double vMin, double aMax, double jMax, std::array& t_brake, std::array& j_brake); + static void get_brake_trajectory(double v0, double a0, double vMax, double vMin, double aMax, double aMin, double jMax, std::array& t_brake, std::array& j_brake); }; @@ -52,14 +52,13 @@ class Step1 { double p0, v0, a0; double pf, vf, af; - double vMax, vMin, aMax, jMax; + double vMax, vMin, aMax, aMin, jMax; // Pre-calculated expressions double pd; double v0_v0, vf_vf; double a0_a0, a0_p3, a0_p4, a0_p5, a0_p6; double af_af, af_p3, af_p4, af_p5, af_p6; - double aMax_aMax; double jMax_jMax; // Only a single velocity-limited profile can be valid @@ -69,23 +68,14 @@ class Step1 { std::array valid_profiles; size_t valid_profile_counter; - void time_up_acc0_acc1_vel(Profile& profile, double vMax, double aMax, double jMax); - void time_up_acc1_vel(Profile& profile, double vMax, double aMax, double jMax); - void time_up_acc0_vel(Profile& profile, double vMax, double aMax, double jMax); - void time_up_vel(Profile& profile, double vMax, double aMax, double jMax); - void time_up_acc0_acc1(Profile& profile, double vMax, double aMax, double jMax); - void time_up_acc1(Profile& profile, double vMax, double aMax, double jMax); - void time_up_acc0(Profile& profile, double vMax, double aMax, double jMax); - void time_up_none(Profile& profile, double vMax, double aMax, double jMax); - - inline void time_down_acc0_acc1_vel(Profile& profile, double vMin, double aMax, double jMax); - inline void time_down_acc1_vel(Profile& profile, double vMin, double aMax, double jMax); - inline void time_down_acc0_vel(Profile& profile, double vMin, double aMax, double jMax); - inline void time_down_vel(Profile& profile, double vMin, double aMax, double jMax); - inline void time_down_acc0_acc1(Profile& profile, double vMin, double aMax, double jMax); - inline void time_down_acc1(Profile& profile, double vMin, double aMax, double jMax); - inline void time_down_acc0(Profile& profile, double vMin, double aMax, double jMax); - inline void time_down_none(Profile& profile, double vMin, double aMax, double jMax); + void time_acc0_acc1_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax); + void time_acc1_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax); + void time_acc0_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax); + void time_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax); + void time_acc0_acc1(Profile& profile, double vMax, double aMax, double aMin, double jMax); + void time_acc1(Profile& profile, double vMax, double aMax, double aMin, double jMax); + void time_acc0(Profile& profile, double vMax, double aMax, double aMin, double jMax); + void time_none(Profile& profile, double vMax, double aMax, double aMin, double jMax); inline void add_profile(Profile profile, double jMax) { profile.direction = (jMax > 0) ? Profile::Direction::UP : Profile::Direction::DOWN; @@ -114,7 +104,7 @@ class Step1 { bool calculate_block(Block& block) const; public: - explicit Step1(double p0, double v0, double a0, double pf, double vf, double af, double vMax, double vMin, double aMax, double jMax); + explicit Step1(double p0, double v0, double a0, double pf, double vf, double af, double vMax, double vMin, double aMax, double aMin, double jMax); bool get_profile(const Profile& input, Block& block); }; @@ -128,7 +118,7 @@ class Step2 { double tf; double p0, v0, a0; double pf, vf, af; - double vMax, vMin, aMax, jMax; + double vMax, vMin, aMax, aMin, jMax; // Pre-calculated expressions double pd; @@ -138,30 +128,31 @@ class Step2 { double v0_v0, vf_vf; double a0_a0, a0_p3, a0_p4, a0_p5, a0_p6; double af_af, af_p3, af_p4, af_p5, af_p6; - double aMax_aMax; double jMax_jMax; double g1, g2; - 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); - - inline bool time_down_acc0_acc1_vel(Profile& profile, double vMin, double aMax, double jMax); - inline bool time_down_acc1_vel(Profile& profile, double vMin, double aMax, double jMax); - inline bool time_down_acc0_vel(Profile& profile, double vMin, double aMax, double jMax); - inline bool time_down_vel(Profile& profile, double vMin, double aMax, double jMax); - inline bool time_down_acc0_acc1(Profile& profile, double vMin, double aMax, double jMax); - inline bool time_down_acc1(Profile& profile, double vMin, double aMax, double jMax); - inline bool time_down_acc0(Profile& profile, double vMin, double aMax, double jMax); - inline bool time_down_none(Profile& profile, double vMin, double aMax, double jMax); + bool time_acc0_acc1_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax); + bool time_acc1_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax); + bool time_acc0_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax); + bool time_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax); + bool time_acc0_acc1(Profile& profile, double vMax, double aMax, double aMin, double jMax); + bool time_acc1(Profile& profile, double vMax, double aMax, double aMin, double jMax); + bool time_acc0(Profile& profile, double vMax, double aMax, double aMin, double jMax); + bool time_none(Profile& profile, double vMax, double aMax, double aMin, double jMax); + + inline bool check_all(Profile& profile, double vMax, double aMax, double aMin, double jMax) { + return time_acc0_acc1_vel(profile, vMax, aMax, aMin, jMax) + || time_acc0_vel(profile, vMax, aMax, aMin, jMax) + || time_acc1_vel(profile, vMax, aMax, aMin, jMax) + || time_vel(profile, vMax, aMax, aMin, jMax) + || time_acc0(profile, vMax, aMax, aMin, jMax) + || time_acc1(profile, vMax, aMax, aMin, jMax) + || time_acc0_acc1(profile, vMax, aMax, aMin, jMax) + || time_none(profile, vMax, aMax, aMin, jMax); + } public: - explicit Step2(double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double vMin, double aMax, double jMax); + explicit Step2(double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double vMin, double aMax, double aMin, double jMax); bool get_profile(Profile& profile); }; diff --git a/include/ruckig/trajectory.hpp b/include/ruckig/trajectory.hpp index 95b8c6ae..cd5f41e0 100644 --- a/include/ruckig/trajectory.hpp +++ b/include/ruckig/trajectory.hpp @@ -38,7 +38,7 @@ struct Profile { std::array t_brakes, j_brakes, a_brakes, v_brakes, p_brakes; template - bool check(double pf, double vf, double af, double jf, double vMax, double aMax) { + bool check(double pf, double vf, double af, double jf, double vMax, double aMax, double aMin) { if (t[0] < 0) { return false; } @@ -81,7 +81,6 @@ struct Profile { } const double vMaxAbs = std::abs(vMax) + 1e-12; - const double aMaxAbs = std::abs(aMax) + 1e-12; for (size_t i = 0; i < 7; ++i) { a[i+1] = a[i] + t[i] * j[i]; @@ -105,29 +104,28 @@ struct Profile { this->jerk_signs = jerk_signs; this->limits = limits; + const double aUppLim = ((aMax > 0) ? aMax : aMin) + 1e-12; + const double aLowLim = ((aMax > 0) ? aMin : aMax) - 1e-12; + // Velocity limit can be broken in the beginning if both initial velocity and acceleration are too high // std::cout << std::setprecision(15) << "target: " << std::abs(p[7]-pf) << " " << std::abs(v[7] - vf) << " " << std::abs(a[7] - af) << " T: " << t_sum[6] << " " << to_string() << std::endl; return std::abs(p[7] - pf) < 1e-8 && std::abs(v[7] - vf) < 1e-8 && std::abs(a[7] - af) < 1e-12 // This is not really needed, but we want to double check - && std::abs(v[3]) < vMaxAbs - && std::abs(v[4]) < vMaxAbs - && std::abs(v[5]) < vMaxAbs - && std::abs(v[6]) < vMaxAbs - && std::abs(a[1]) < aMaxAbs - && std::abs(a[3]) < aMaxAbs - && std::abs(a[5]) < aMaxAbs; + && std::abs(v[3]) < vMaxAbs && std::abs(v[4]) < vMaxAbs && std::abs(v[5]) < vMaxAbs && std::abs(v[6]) < vMaxAbs + && a[1] > aLowLim && a[3] > aLowLim && a[5] > aLowLim + && a[1] < aUppLim && a[3] < aUppLim && a[5] < aUppLim; } template - inline bool check([[maybe_unused]] double tf, double pf, double vf, double af, double jf, double vMax, double aMax) { + inline bool check([[maybe_unused]] double tf, double pf, double vf, double af, double jf, double vMax, double aMax, double aMin) { // Time doesn't need to be checked as every profile has a: tf - ... equation - return check(pf, vf, af, jf, vMax, aMax); // && (std::abs(t_sum[6] - tf) < 1e-8); + return check(pf, vf, af, jf, vMax, aMax, aMin); // && (std::abs(t_sum[6] - tf) < 1e-8); } template - inline bool check(double tf, double pf, double vf, double af, double jf, double vMax, double aMax, double jMax) { - return (std::abs(jf) < std::abs(jMax) + 1e-12) && check(tf, pf, vf, af, jf, vMax, aMax); + inline bool check(double tf, double pf, double vf, double af, double jf, double vMax, double aMax, double aMin, double jMax) { + return (std::abs(jf) < std::abs(jMax) + 1e-12) && check(tf, pf, vf, af, jf, vMax, aMax, aMin); } //! Integrate with constant jerk for duration t. Returns new position, new velocity, and new acceleration. diff --git a/notebooks/ruckig-step1-uddu.nb b/notebooks/ruckig-step1-uddu.nb index 2800283c..e2e9a21f 100644 --- a/notebooks/ruckig-step1-uddu.nb +++ b/notebooks/ruckig-step1-uddu.nb @@ -10,10 +10,10 @@ NotebookFileLineBreakTest NotebookFileLineBreakTest NotebookDataPosition[ 158, 7] -NotebookDataLength[ 197264, 5176] -NotebookOptionsPosition[ 187187, 5009] -NotebookOutlinePosition[ 187583, 5025] -CellTagsIndexPosition[ 187540, 5022] +NotebookDataLength[ 194821, 5124] +NotebookOptionsPosition[ 184741, 4957] +NotebookOutlinePosition[ 185141, 4973] +CellTagsIndexPosition[ 185098, 4970] WindowFrame->Normal*) (* Beginning of Notebook Content *) @@ -239,8 +239,7 @@ Cell[BoxData[{ 3.817631310598453*^9, 3.817640134759371*^9, 3.818009215253849*^9, { 3.819365329290554*^9, 3.819365331663157*^9}, 3.820148038073226*^9, { 3.8205597423263273`*^9, 3.820559744425127*^9}}, - CellLabel-> - "In[2236]:=",ExpressionUUID->"d346b209-9dd3-47cf-8f1e-f04bc3bf980a"], + CellLabel->"In[1]:=",ExpressionUUID->"d346b209-9dd3-47cf-8f1e-f04bc3bf980a"], Cell[BoxData[ RowBox[{ @@ -256,8 +255,7 @@ Cell[BoxData[ RowBox[{"p7", "\[Equal]", "pf"}], ",", RowBox[{"a3", "\[Equal]", "0"}], ",", RowBox[{"a1", "\[Equal]", "aMax"}], ",", - RowBox[{"a5", "\[Equal]", - RowBox[{"-", "aMax"}]}], ",", + RowBox[{"a5", "\[Equal]", "aMin"}], ",", RowBox[{"v3", "\[Equal]", "vMax"}]}], "}"}], ",", "tVars"}], "]"}], ",", "\[IndentingNewLine]", RowBox[{"Solve", "[", @@ -269,8 +267,7 @@ Cell[BoxData[ RowBox[{"p7", "\[Equal]", "pf"}], ",", RowBox[{"a3", "\[Equal]", "0"}], ",", RowBox[{"a1", "\[Equal]", "aMax"}], ",", - RowBox[{"a5", "\[Equal]", - RowBox[{"-", "aMax"}]}], ",", + RowBox[{"a5", "\[Equal]", "aMin"}], ",", RowBox[{"t4", "\[Equal]", "0"}]}], "}"}], ",", "tVars"}], "]"}], ",", "\[IndentingNewLine]", RowBox[{"Solve", "[", @@ -282,8 +279,7 @@ Cell[BoxData[ RowBox[{"p7", "\[Equal]", "pf"}], ",", RowBox[{"a3", "\[Equal]", "0"}], ",", RowBox[{"t2", "\[Equal]", "0"}], ",", - RowBox[{"a5", "\[Equal]", - RowBox[{"-", "aMax"}]}], ",", + RowBox[{"a5", "\[Equal]", "aMin"}], ",", RowBox[{"v3", "\[Equal]", "vMax"}]}], "}"}], ",", "tVars"}], "]"}], ",", "\[IndentingNewLine]", RowBox[{"Solve", "[", @@ -438,9 +434,9 @@ Cell[BoxData[ 3.817629903651003*^9, 3.81762990789653*^9}, 3.817630008980739*^9, { 3.8176308336239557`*^9, 3.8176308355038147`*^9}, 3.8176412855214357`*^9, { 3.819480271236746*^9, 3.819480275524906*^9}, {3.820559923457724*^9, - 3.820559934200976*^9}, {3.820560449143613*^9, 3.820560470192191*^9}}, - CellLabel-> - "In[2397]:=",ExpressionUUID->"ccba75f7-892e-4119-93ec-41811b2e330e"], + 3.820559934200976*^9}, {3.820560449143613*^9, 3.820560470192191*^9}, { + 3.821871609794586*^9, 3.821871622909561*^9}}, + CellLabel->"In[25]:=",ExpressionUUID->"ccba75f7-892e-4119-93ec-41811b2e330e"], Cell["\<\ Information Roots are max. 4th Order @@ -489,7 +485,7 @@ Cell[BoxData[{ RowBox[{ RowBox[{"tmpResult", "=", RowBox[{"resultT", "[", - RowBox[{"[", "11", "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"[", "2", "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", RowBox[{"\"\\"", "<>", RowBox[{"ToString", "[", RowBox[{"Length", "[", "tmpResult", "]"}], @@ -500,7 +496,7 @@ Cell[BoxData[{ RowBox[{"Refine", "[", RowBox[{"tmpResult", "[", RowBox[{"[", - RowBox[{"2", ",", "2", ",", "2"}], "]"}], "]"}], "]"}], ",", + RowBox[{"2", ",", "6", ",", "2"}], "]"}], "]"}], "]"}], ",", RowBox[{"{", RowBox[{ RowBox[{"vMax", "\[Element]", "Reals"}], ",", @@ -511,22 +507,18 @@ Cell[BoxData[{ RowBox[{"ToString", "[", RowBox[{"tmp", ",", "CForm"}], "]"}], ";"}], "\[IndentingNewLine]", RowBox[{"CopyToClipboard", "[", "%", "]"}]}], "Input", - CellChangeTimes->{{3.817463498279006*^9, 3.8174635541359243`*^9}, { - 3.817463724470017*^9, 3.817463789974394*^9}, {3.817464378120303*^9, - 3.817464386126404*^9}, {3.8174663536199083`*^9, 3.817466375443508*^9}, { - 3.81746640558984*^9, 3.8174664235008507`*^9}, {3.817466533768512*^9, - 3.8174665338076363`*^9}, {3.817466573419918*^9, 3.817466635930318*^9}, { - 3.817466697678646*^9, 3.8174667573661222`*^9}, {3.817466808377346*^9, - 3.817466863761118*^9}, {3.817466903140582*^9, 3.817466947127493*^9}, { - 3.817467040839796*^9, 3.817467216640294*^9}, {3.817467388620803*^9, - 3.817467475166068*^9}, {3.817629429937437*^9, 3.817629431528776*^9}, { - 3.817629486355568*^9, 3.8176295372755327`*^9}, {3.8176412523276567`*^9, - 3.8176413898972397`*^9}, {3.817700834366179*^9, 3.8177008735097103`*^9}, { - 3.820559978939802*^9, 3.82055998400837*^9}}, + CellChangeTimes->CompressedData[" +1:eJxTTMoPSmViYGAQAWIQPe+tx03rwjeOdZEuD0B0Ilf/VxBto7f/L4huu+ns +bgOkG7hdPEC01zVjO1sgbWH4zgFE/7i/yQVMs31wA9HLpu8IAdEaN3aC6R/5 +ayNBtNbR2kQQXbHdNBNEu21fXQA2R5inHESLRP+oBeuvf9IEonNEUjpA9Jz7 +UlNAtFetyCoQPe/u5P0gOi8g9RSIrvm/914PkN7z+gOYvpBw+imI1rqo8QZM +13QtmQSkDbbt2Qqi92733rMeSPf4GxwE0XeE4s4ql79xnOjCcA5EG53kdFpS ++cZRZo6dN4h+1SaVCKL3cTHlguhu1V9FILqwYVoJiAYAKjqh/w== + "], CellLabel-> - "In[2311]:=",ExpressionUUID->"f20278fb-5b15-4c9c-bd38-9f1d3931e2f3"], + "In[161]:=",ExpressionUUID->"f20278fb-5b15-4c9c-bd38-9f1d3931e2f3"], -Cell[BoxData["\<\"Solutions: 4\"\>"], "Output", +Cell[BoxData["\<\"Solutions: 2\"\>"], "Output", CellChangeTimes->{{3.817463508059903*^9, 3.817463555229458*^9}, 3.817463726805722*^9, {3.817463761974618*^9, 3.81746379044567*^9}, { 3.817464381894802*^9, 3.8174643873252087`*^9}, {3.8174663563162394`*^9, @@ -538,143 +530,97 @@ Cell[BoxData["\<\"Solutions: 4\"\>"], "Output", 3.817467389302877*^9, 3.8174674761335993`*^9}, 3.8176294327019367`*^9, { 3.817629487238022*^9, 3.8176295375030212`*^9}, {3.817641270569572*^9, 3.817641390401013*^9}, {3.817700838931745*^9, 3.817700873737822*^9}, { - 3.820559979771512*^9, 3.820559984275301*^9}}, + 3.820559979771512*^9, 3.820559984275301*^9}, {3.8218716334083567`*^9, + 3.8218717064697647`*^9}, {3.821871882114599*^9, 3.821871976452033*^9}, { + 3.821872025084113*^9, 3.821872037057416*^9}}, CellLabel-> - "Out[2312]=",ExpressionUUID->"b8542058-8604-4109-afce-1ce2cbfeefa3"], + "Out[162]=",ExpressionUUID->"4cd7848d-fdf9-4f4e-b495-1bf7e99dc1bd"], Cell[BoxData[ RowBox[{ FractionBox["1", - RowBox[{"2", " ", "aMax", " ", "jMax"}]], + RowBox[{"6", " ", + RowBox[{"(", + RowBox[{"aMax", "-", "aMin"}], ")"}], " ", "aMin", " ", + SuperscriptBox["jMax", "2"]}]], RowBox[{"(", RowBox[{ - SuperscriptBox["a0", "2"], "-", - SuperscriptBox["af", "2"], "-", - RowBox[{"2", " ", "jMax", " ", "v0"}], "+", - RowBox[{"2", " ", "jMax", " ", "vf"}], "-", - RowBox[{"4", " ", "aMax", " ", "jMax", " ", - RowBox[{"Root", "[", + RowBox[{ + RowBox[{"-", "3"}], " ", + RowBox[{"(", + RowBox[{"aMax", "-", "aMin"}], ")"}], " ", "jMax", " ", + RowBox[{"(", RowBox[{ - RowBox[{ - RowBox[{ - RowBox[{ - RowBox[{"-", "3"}], " ", - SuperscriptBox["a0", "4"]}], "+", - RowBox[{"3", " ", - SuperscriptBox["af", "4"]}], "+", - RowBox[{"8", " ", - SuperscriptBox["a0", "3"], " ", "aMax"}], "-", - RowBox[{"8", " ", - SuperscriptBox["af", "3"], " ", "aMax"}], "-", - RowBox[{"6", " ", - SuperscriptBox["a0", "2"], " ", - SuperscriptBox["aMax", "2"]}], "+", - RowBox[{"6", " ", - SuperscriptBox["af", "2"], " ", - SuperscriptBox["aMax", "2"]}], "+", - RowBox[{"24", " ", "aMax", " ", - SuperscriptBox["jMax", "2"], " ", "p0"}], "-", - RowBox[{"24", " ", "aMax", " ", - SuperscriptBox["jMax", "2"], " ", "pf"}], "+", - RowBox[{"12", " ", - SuperscriptBox["a0", "2"], " ", "jMax", " ", "v0"}], "-", - RowBox[{"24", " ", "a0", " ", "aMax", " ", "jMax", " ", "v0"}], "+", - - RowBox[{"12", " ", - SuperscriptBox["aMax", "2"], " ", "jMax", " ", "v0"}], "-", - RowBox[{"12", " ", - SuperscriptBox["jMax", "2"], " ", - SuperscriptBox["v0", "2"]}], "-", - RowBox[{"12", " ", - SuperscriptBox["af", "2"], " ", "jMax", " ", "vf"}], "+", - RowBox[{"24", " ", "af", " ", "aMax", " ", "jMax", " ", "vf"}], "-", - - RowBox[{"12", " ", - SuperscriptBox["aMax", "2"], " ", "jMax", " ", "vf"}], "+", - RowBox[{"12", " ", - SuperscriptBox["jMax", "2"], " ", - SuperscriptBox["vf", "2"]}], "+", + SuperscriptBox["af", "2"], "+", + RowBox[{"aMax", " ", "aMin"}], "-", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "12"}], " ", - SuperscriptBox["af", "2"], " ", - SuperscriptBox["jMax", "2"]}], "+", - RowBox[{"12", " ", - SuperscriptBox["aMax", "2"], " ", - SuperscriptBox["jMax", "2"]}], "+", - RowBox[{"24", " ", - SuperscriptBox["jMax", "3"], " ", "vf"}]}], ")"}], " ", - SuperscriptBox["#1", "2"]}], "-", - RowBox[{"24", " ", "aMax", " ", - SuperscriptBox["jMax", "3"], " ", - SuperscriptBox["#1", "3"]}], "+", - RowBox[{"12", " ", - SuperscriptBox["jMax", "4"], " ", - SuperscriptBox["#1", "4"]}]}], "&"}], ",", "2"}], "]"}]}], "+", - RowBox[{"2", " ", - SuperscriptBox["jMax", "2"], " ", - SuperscriptBox[ - RowBox[{"Root", "[", + SuperscriptBox["aMin", "2"], "+", + RowBox[{"jMax", " ", "vf"}]}], ")"}]}]}], ")"}]}], "+", + RowBox[{ + SqrtBox["3"], " ", + RowBox[{"\[Sqrt]", + RowBox[{"(", RowBox[{ - RowBox[{ + RowBox[{"(", + RowBox[{"aMax", "-", "aMin"}], ")"}], " ", + RowBox[{"(", RowBox[{ - RowBox[{ - RowBox[{"-", "3"}], " ", - SuperscriptBox["a0", "4"]}], "+", RowBox[{"3", " ", - SuperscriptBox["af", "4"]}], "+", + SuperscriptBox["af", "4"], " ", "aMax"}], "-", + RowBox[{"3", " ", + SuperscriptBox["a0", "4"], " ", "aMin"}], "+", RowBox[{"8", " ", - SuperscriptBox["a0", "3"], " ", "aMax"}], "-", + SuperscriptBox["a0", "3"], " ", "aMax", " ", "aMin"}], "-", RowBox[{"8", " ", - SuperscriptBox["af", "3"], " ", "aMax"}], "-", - RowBox[{"6", " ", - SuperscriptBox["a0", "2"], " ", - SuperscriptBox["aMax", "2"]}], "+", + SuperscriptBox["af", "3"], " ", "aMax", " ", "aMin"}], "-", + RowBox[{ + "24", " ", "a0", " ", "aMax", " ", "aMin", " ", "jMax", " ", "v0"}], + "-", RowBox[{"6", " ", - SuperscriptBox["af", "2"], " ", - SuperscriptBox["aMax", "2"]}], "+", - RowBox[{"24", " ", "aMax", " ", - SuperscriptBox["jMax", "2"], " ", "p0"}], "-", - RowBox[{"24", " ", "aMax", " ", - SuperscriptBox["jMax", "2"], " ", "pf"}], "+", - RowBox[{"12", " ", - SuperscriptBox["a0", "2"], " ", "jMax", " ", "v0"}], "-", - RowBox[{"24", " ", "a0", " ", "aMax", " ", "jMax", " ", "v0"}], "+", - RowBox[{"12", " ", - SuperscriptBox["aMax", "2"], " ", "jMax", " ", "v0"}], "-", - RowBox[{"12", " ", - SuperscriptBox["jMax", "2"], " ", - SuperscriptBox["v0", "2"]}], "-", - RowBox[{"12", " ", - SuperscriptBox["af", "2"], " ", "jMax", " ", "vf"}], "+", - RowBox[{"24", " ", "af", " ", "aMax", " ", "jMax", " ", "vf"}], "-", - RowBox[{"12", " ", - SuperscriptBox["aMax", "2"], " ", "jMax", " ", "vf"}], "+", - RowBox[{"12", " ", - SuperscriptBox["jMax", "2"], " ", - SuperscriptBox["vf", "2"]}], "+", + SuperscriptBox["a0", "2"], " ", "aMin", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["aMax", "2"], "-", + RowBox[{"2", " ", "jMax", " ", "v0"}]}], ")"}]}], "+", RowBox[{ + "24", " ", "af", " ", "aMax", " ", "aMin", " ", "jMax", " ", "vf"}], + "+", + RowBox[{"6", " ", + SuperscriptBox["af", "2"], " ", "aMax", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["aMin", "2"], "-", + RowBox[{"2", " ", "jMax", " ", "vf"}]}], ")"}]}], "+", + RowBox[{"3", " ", RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "12"}], " ", - SuperscriptBox["af", "2"], " ", - SuperscriptBox["jMax", "2"]}], "+", - RowBox[{"12", " ", + SuperscriptBox["aMax", "3"], " ", + SuperscriptBox["aMin", "2"]}], "-", + RowBox[{"4", " ", "aMin", " ", + SuperscriptBox["jMax", "2"], " ", + SuperscriptBox["v0", "2"]}], "-", + RowBox[{ SuperscriptBox["aMax", "2"], " ", - SuperscriptBox["jMax", "2"]}], "+", - RowBox[{"24", " ", - SuperscriptBox["jMax", "3"], " ", "vf"}]}], ")"}], " ", - SuperscriptBox["#1", "2"]}], "-", - RowBox[{"24", " ", "aMax", " ", - SuperscriptBox["jMax", "3"], " ", - SuperscriptBox["#1", "3"]}], "+", - RowBox[{"12", " ", - SuperscriptBox["jMax", "4"], " ", - SuperscriptBox["#1", "4"]}]}], "&"}], ",", "2"}], "]"}], "2"]}]}], - ")"}]}]], "Output", + RowBox[{"(", + RowBox[{ + SuperscriptBox["aMin", "3"], "-", + RowBox[{"4", " ", "aMin", " ", "jMax", " ", "v0"}]}], ")"}]}], + "+", + RowBox[{"4", " ", "aMax", " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "aMin", " ", "jMax", " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf"}], ")"}]}], "-", + RowBox[{ + SuperscriptBox["aMin", "2"], " ", "vf"}], "+", + RowBox[{"jMax", " ", + SuperscriptBox["vf", "2"]}]}], ")"}]}]}], ")"}]}]}], ")"}]}], + ")"}]}], " ", + RowBox[{"Abs", "[", "jMax", "]"}]}]}], ")"}]}]], "Output", CellChangeTimes->{{3.817463508059903*^9, 3.817463555229458*^9}, 3.817463726805722*^9, {3.817463761974618*^9, 3.81746379044567*^9}, { 3.817464381894802*^9, 3.8174643873252087`*^9}, {3.8174663563162394`*^9, @@ -686,9 +632,11 @@ Cell[BoxData[ 3.817467389302877*^9, 3.8174674761335993`*^9}, 3.8176294327019367`*^9, { 3.817629487238022*^9, 3.8176295375030212`*^9}, {3.817641270569572*^9, 3.817641390401013*^9}, {3.817700838931745*^9, 3.817700873737822*^9}, { - 3.820559979771512*^9, 3.820559984303294*^9}}, + 3.820559979771512*^9, 3.820559984275301*^9}, {3.8218716334083567`*^9, + 3.8218717064697647`*^9}, {3.821871882114599*^9, 3.821871976452033*^9}, { + 3.821872025084113*^9, 3.821872037181758*^9}}, CellLabel-> - "Out[2313]=",ExpressionUUID->"28843d46-3373-4bdd-af5c-1574cef3ab3b"] + "Out[163]=",ExpressionUUID->"72d60704-2911-4ccb-8d4c-6119a0a66aa5"] }, Open ]], Cell["Print Cases 6-8: Roots", "Text", @@ -5008,8 +4956,8 @@ Cell[BoxData[ }, Open ]] }, WindowSize->{918, 888}, -WindowMargins->{{Automatic, 165}, {-255, Automatic}}, -FrontEndVersion->"12.1 for Mac OS X x86 (64-bit) (June 19, 2020)", +WindowMargins->{{Automatic, -40}, {-153, Automatic}}, +FrontEndVersion->"12.2 for Mac OS X x86 (64-bit) (December 12, 2020)", StyleDefinitions->"Default.nb", ExpressionUUID->"c8b287f8-8b4a-4e98-ab1e-df7dcd999be5" ] @@ -5025,158 +4973,158 @@ CellTagsIndex->{} (*NotebookFileOutline Notebook[{ Cell[558, 20, 206, 4, 35, "Text",ExpressionUUID->"f0efc888-777d-4c66-b8e6-2db88daacc31"], -Cell[767, 26, 6942, 216, 812, "Input",ExpressionUUID->"d346b209-9dd3-47cf-8f1e-f04bc3bf980a"], -Cell[7712, 244, 8163, 198, 430, "Input",ExpressionUUID->"ccba75f7-892e-4119-93ec-41811b2e330e"], -Cell[15878, 444, 1694, 30, 311, "Text",ExpressionUUID->"e82cb2e9-b4f4-4bc2-9a74-547157d3c4dc"], -Cell[17575, 476, 249, 7, 58, "Text",ExpressionUUID->"2bb13fa1-f9ef-4a5d-ac3b-101e020ad421"], +Cell[767, 26, 6936, 215, 812, "Input",ExpressionUUID->"d346b209-9dd3-47cf-8f1e-f04bc3bf980a"], +Cell[7706, 243, 8130, 195, 430, "Input",ExpressionUUID->"ccba75f7-892e-4119-93ec-41811b2e330e"], +Cell[15839, 440, 1694, 30, 311, "Text",ExpressionUUID->"e82cb2e9-b4f4-4bc2-9a74-547157d3c4dc"], +Cell[17536, 472, 249, 7, 58, "Text",ExpressionUUID->"2bb13fa1-f9ef-4a5d-ac3b-101e020ad421"], Cell[CellGroupData[{ -Cell[17849, 487, 1846, 39, 115, "Input",ExpressionUUID->"f20278fb-5b15-4c9c-bd38-9f1d3931e2f3"], -Cell[19698, 528, 993, 14, 34, "Output",ExpressionUUID->"b8542058-8604-4109-afce-1ce2cbfeefa3"], -Cell[20694, 544, 6320, 146, 230, "Output",ExpressionUUID->"28843d46-3373-4bdd-af5c-1574cef3ab3b"] +Cell[17810, 483, 1394, 35, 115, "Input",ExpressionUUID->"f20278fb-5b15-4c9c-bd38-9f1d3931e2f3"], +Cell[19207, 520, 1142, 16, 34, "Output",ExpressionUUID->"4cd7848d-fdf9-4f4e-b495-1bf7e99dc1bd"], +Cell[20352, 538, 4216, 100, 149, "Output",ExpressionUUID->"72d60704-2911-4ccb-8d4c-6119a0a66aa5"] }, Open ]], -Cell[27029, 693, 166, 3, 35, "Text",ExpressionUUID->"8d7af2fe-a47e-4ef4-b28a-db0dc764dd0f"], +Cell[24583, 641, 166, 3, 35, "Text",ExpressionUUID->"8d7af2fe-a47e-4ef4-b28a-db0dc764dd0f"], Cell[CellGroupData[{ -Cell[27220, 700, 2231, 51, 159, "Input",ExpressionUUID->"ca8158ca-2cc6-4c2b-92b0-ae9728cb14cd"], -Cell[29454, 753, 2428, 59, 79, "Output",ExpressionUUID->"0a94f786-574e-4852-9434-e6c9207268ec"] +Cell[24774, 648, 2231, 51, 159, "Input",ExpressionUUID->"ca8158ca-2cc6-4c2b-92b0-ae9728cb14cd"], +Cell[27008, 701, 2428, 59, 79, "Output",ExpressionUUID->"0a94f786-574e-4852-9434-e6c9207268ec"] }, Open ]], Cell[CellGroupData[{ -Cell[31919, 817, 1425, 28, 94, "Input",ExpressionUUID->"02ddacce-ea40-40b5-bbc9-e4e0e8a8500b"], -Cell[33347, 847, 990, 16, 54, "Output",ExpressionUUID->"cd5c1068-df37-4f84-8783-2e31418f59b2"] +Cell[29473, 765, 1425, 28, 94, "Input",ExpressionUUID->"02ddacce-ea40-40b5-bbc9-e4e0e8a8500b"], +Cell[30901, 795, 990, 16, 54, "Output",ExpressionUUID->"cd5c1068-df37-4f84-8783-2e31418f59b2"] }, Open ]], Cell[CellGroupData[{ -Cell[34374, 868, 1671, 37, 94, "Input",ExpressionUUID->"bbf1a9de-0221-4992-b9c6-d1f769287d62"], -Cell[36048, 907, 2462, 53, 98, "Output",ExpressionUUID->"105051c1-dd97-49a1-a1ea-1856a027794e"], -Cell[38513, 962, 829, 13, 34, "Output",ExpressionUUID->"ee561e7d-9711-4560-80d9-d30648c0977e"] +Cell[31928, 816, 1671, 37, 94, "Input",ExpressionUUID->"bbf1a9de-0221-4992-b9c6-d1f769287d62"], +Cell[33602, 855, 2462, 53, 98, "Output",ExpressionUUID->"105051c1-dd97-49a1-a1ea-1856a027794e"], +Cell[36067, 910, 829, 13, 34, "Output",ExpressionUUID->"ee561e7d-9711-4560-80d9-d30648c0977e"] }, Open ]], -Cell[39357, 978, 217, 4, 38, "CodeText",ExpressionUUID->"3f45e156-5cf9-44e3-af10-aa1a13b499e9"], -Cell[39577, 984, 283, 8, 30, "Input",ExpressionUUID->"e818af86-de53-4897-8691-2be2441cd16a"], +Cell[36911, 926, 217, 4, 38, "CodeText",ExpressionUUID->"3f45e156-5cf9-44e3-af10-aa1a13b499e9"], +Cell[37131, 932, 283, 8, 30, "Input",ExpressionUUID->"e818af86-de53-4897-8691-2be2441cd16a"], Cell[CellGroupData[{ -Cell[39885, 996, 2722, 75, 115, "Input",ExpressionUUID->"d8d6b366-35dd-4052-aa2e-094a5ea14d30"], -Cell[42610, 1073, 806, 16, 53, "Output",ExpressionUUID->"c9cf3bd0-d0c1-4283-a30b-fc90574f3520"], -Cell[43419, 1091, 823, 16, 54, "Output",ExpressionUUID->"8333c215-0a37-4c28-a603-fcd19508196a"], -Cell[44245, 1109, 1150, 24, 56, "Output",ExpressionUUID->"70f967be-3b0e-42d6-b07b-5d0c463699fc"] +Cell[37439, 944, 2722, 75, 115, "Input",ExpressionUUID->"d8d6b366-35dd-4052-aa2e-094a5ea14d30"], +Cell[40164, 1021, 806, 16, 53, "Output",ExpressionUUID->"c9cf3bd0-d0c1-4283-a30b-fc90574f3520"], +Cell[40973, 1039, 823, 16, 54, "Output",ExpressionUUID->"8333c215-0a37-4c28-a603-fcd19508196a"], +Cell[41799, 1057, 1150, 24, 56, "Output",ExpressionUUID->"70f967be-3b0e-42d6-b07b-5d0c463699fc"] }, Open ]], Cell[CellGroupData[{ -Cell[45432, 1138, 639, 17, 73, "Input",ExpressionUUID->"ba86318c-49c6-475e-8175-23998c1d35c0"], -Cell[46074, 1157, 317, 7, 54, "Output",ExpressionUUID->"d47918f5-d1be-4a71-b5a6-ecb86447e980"], -Cell[46394, 1166, 262, 4, 34, "Output",ExpressionUUID->"fb288fdc-c395-407c-abb8-789580173713"] +Cell[42986, 1086, 639, 17, 73, "Input",ExpressionUUID->"ba86318c-49c6-475e-8175-23998c1d35c0"], +Cell[43628, 1105, 317, 7, 54, "Output",ExpressionUUID->"d47918f5-d1be-4a71-b5a6-ecb86447e980"], +Cell[43948, 1114, 262, 4, 34, "Output",ExpressionUUID->"fb288fdc-c395-407c-abb8-789580173713"] }, Open ]], Cell[CellGroupData[{ -Cell[46693, 1175, 1379, 39, 159, "Input",ExpressionUUID->"8661a96e-a382-4a1f-9033-993cef60d8c3"], -Cell[48075, 1216, 1780, 51, 79, "Output",ExpressionUUID->"26f01fc3-d112-4df2-a15e-79678a1bc775"] +Cell[44247, 1123, 1379, 39, 159, "Input",ExpressionUUID->"8661a96e-a382-4a1f-9033-993cef60d8c3"], +Cell[45629, 1164, 1780, 51, 79, "Output",ExpressionUUID->"26f01fc3-d112-4df2-a15e-79678a1bc775"] }, Open ]], Cell[CellGroupData[{ -Cell[49892, 1272, 2109, 65, 94, "Input",ExpressionUUID->"d57e959b-b7e7-4c01-a8fa-026edffa41d9"], -Cell[52004, 1339, 516, 13, 54, "Output",ExpressionUUID->"b11919c9-3144-4341-aba1-0b1d8be41a19"], -Cell[52523, 1354, 432, 11, 54, "Output",ExpressionUUID->"1c12974d-0498-476d-927d-13b39c32b6c2"], -Cell[52958, 1367, 936, 23, 56, "Output",ExpressionUUID->"f685fef8-e488-40f3-869c-d77499e633dc"] +Cell[47446, 1220, 2109, 65, 94, "Input",ExpressionUUID->"d57e959b-b7e7-4c01-a8fa-026edffa41d9"], +Cell[49558, 1287, 516, 13, 54, "Output",ExpressionUUID->"b11919c9-3144-4341-aba1-0b1d8be41a19"], +Cell[50077, 1302, 432, 11, 54, "Output",ExpressionUUID->"1c12974d-0498-476d-927d-13b39c32b6c2"], +Cell[50512, 1315, 936, 23, 56, "Output",ExpressionUUID->"f685fef8-e488-40f3-869c-d77499e633dc"] }, Open ]], Cell[CellGroupData[{ -Cell[53931, 1395, 1544, 42, 73, "Input",ExpressionUUID->"c031ee14-cf99-44c4-b969-5d49c2e072bf"], -Cell[55478, 1439, 474, 10, 34, "Output",ExpressionUUID->"83f74dbf-bf61-4c36-b5da-59096000bc76"] +Cell[51485, 1343, 1544, 42, 73, "Input",ExpressionUUID->"c031ee14-cf99-44c4-b969-5d49c2e072bf"], +Cell[53032, 1387, 474, 10, 34, "Output",ExpressionUUID->"83f74dbf-bf61-4c36-b5da-59096000bc76"] }, Open ]], Cell[CellGroupData[{ -Cell[55989, 1454, 584, 15, 73, "Input",ExpressionUUID->"32f421af-9488-445c-9a56-fcc5c55c6037"], -Cell[56576, 1471, 791, 19, 56, "Output",ExpressionUUID->"e9690900-44cb-4a97-85a3-7c8e1662421c"], -Cell[57370, 1492, 416, 6, 56, "Output",ExpressionUUID->"846e2b30-d6d0-4e57-b203-3ccfaa6c2c9f"] +Cell[53543, 1402, 584, 15, 73, "Input",ExpressionUUID->"32f421af-9488-445c-9a56-fcc5c55c6037"], +Cell[54130, 1419, 791, 19, 56, "Output",ExpressionUUID->"e9690900-44cb-4a97-85a3-7c8e1662421c"], +Cell[54924, 1440, 416, 6, 56, "Output",ExpressionUUID->"846e2b30-d6d0-4e57-b203-3ccfaa6c2c9f"] }, Open ]], Cell[CellGroupData[{ -Cell[57823, 1503, 571, 16, 30, "Input",ExpressionUUID->"98e5f169-5f1d-47e0-b55e-2ae66f4200cb"], -Cell[58397, 1521, 18246, 501, 762, "Output",ExpressionUUID->"8ee6fb37-c85f-48c4-aa14-1794c1a3c7c6"] +Cell[55377, 1451, 571, 16, 30, "Input",ExpressionUUID->"98e5f169-5f1d-47e0-b55e-2ae66f4200cb"], +Cell[55951, 1469, 18246, 501, 762, "Output",ExpressionUUID->"8ee6fb37-c85f-48c4-aa14-1794c1a3c7c6"] }, Open ]], -Cell[76658, 2025, 3630, 92, 214, "Input",ExpressionUUID->"6e919ab8-7fe2-47c6-90c0-23d9f010233f"], +Cell[74212, 1973, 3630, 92, 214, "Input",ExpressionUUID->"6e919ab8-7fe2-47c6-90c0-23d9f010233f"], Cell[CellGroupData[{ -Cell[80313, 2121, 800, 23, 30, "Input",ExpressionUUID->"48585b25-f917-4a21-8579-f955faf3196d"], -Cell[81116, 2146, 5214, 126, 258, "Output",ExpressionUUID->"86b769ae-d0e9-46d5-bec7-0ff43a85c15b"] +Cell[77867, 2069, 800, 23, 30, "Input",ExpressionUUID->"48585b25-f917-4a21-8579-f955faf3196d"], +Cell[78670, 2094, 5214, 126, 258, "Output",ExpressionUUID->"86b769ae-d0e9-46d5-bec7-0ff43a85c15b"] }, Open ]], Cell[CellGroupData[{ -Cell[86367, 2277, 1652, 44, 73, "Input",ExpressionUUID->"f0e719fb-1f43-4b7b-ab13-e0030e29f802"], -Cell[88022, 2323, 389, 6, 34, "Output",ExpressionUUID->"39d1a936-3d0f-4fbf-8715-c719389e4289"] +Cell[83921, 2225, 1652, 44, 73, "Input",ExpressionUUID->"f0e719fb-1f43-4b7b-ab13-e0030e29f802"], +Cell[85576, 2271, 389, 6, 34, "Output",ExpressionUUID->"39d1a936-3d0f-4fbf-8715-c719389e4289"] }, Open ]], -Cell[88426, 2332, 4103, 101, 244, "Input",ExpressionUUID->"deec1acf-7f7f-498a-a271-e1d2a4884e2c"], +Cell[85980, 2280, 4103, 101, 244, "Input",ExpressionUUID->"deec1acf-7f7f-498a-a271-e1d2a4884e2c"], Cell[CellGroupData[{ -Cell[92554, 2437, 3624, 104, 157, "Input",ExpressionUUID->"76c16481-de84-4e2e-a943-429ec44acad1"], -Cell[96181, 2543, 1071, 20, 54, "Output",ExpressionUUID->"4675b0ed-5411-4e27-b784-ece8f684f923"], -Cell[97255, 2565, 987, 18, 54, "Output",ExpressionUUID->"eb011791-010b-4a86-a2e5-3017cc81a2b0"], -Cell[98245, 2585, 1491, 30, 56, "Output",ExpressionUUID->"68ed0252-2b59-470b-8053-401273051ec8"], -Cell[99739, 2617, 1292, 29, 64, "Output",ExpressionUUID->"be5f1406-07e7-49d2-b86b-8e78cb1713a9"] +Cell[90108, 2385, 3624, 104, 157, "Input",ExpressionUUID->"76c16481-de84-4e2e-a943-429ec44acad1"], +Cell[93735, 2491, 1071, 20, 54, "Output",ExpressionUUID->"4675b0ed-5411-4e27-b784-ece8f684f923"], +Cell[94809, 2513, 987, 18, 54, "Output",ExpressionUUID->"eb011791-010b-4a86-a2e5-3017cc81a2b0"], +Cell[95799, 2533, 1491, 30, 56, "Output",ExpressionUUID->"68ed0252-2b59-470b-8053-401273051ec8"], +Cell[97293, 2565, 1292, 29, 64, "Output",ExpressionUUID->"be5f1406-07e7-49d2-b86b-8e78cb1713a9"] }, Open ]], Cell[CellGroupData[{ -Cell[101068, 2651, 708, 22, 30, "Input",ExpressionUUID->"861d34e3-2181-41b8-9a73-9af562bed505"], -Cell[101779, 2675, 694, 19, 56, "Output",ExpressionUUID->"a0048a6d-ac5f-4c36-a7f8-8a7117a4ed35"] +Cell[98622, 2599, 708, 22, 30, "Input",ExpressionUUID->"861d34e3-2181-41b8-9a73-9af562bed505"], +Cell[99333, 2623, 694, 19, 56, "Output",ExpressionUUID->"a0048a6d-ac5f-4c36-a7f8-8a7117a4ed35"] }, Open ]], Cell[CellGroupData[{ -Cell[102510, 2699, 936, 29, 52, "Input",ExpressionUUID->"8ce61ef8-3d6a-4ec6-b070-7abcd1cfb2c8"], -Cell[103449, 2730, 221, 6, 52, "Output",ExpressionUUID->"fdecf7ce-a690-4d2a-90bb-ba63fe9a6577"] +Cell[100064, 2647, 936, 29, 52, "Input",ExpressionUUID->"8ce61ef8-3d6a-4ec6-b070-7abcd1cfb2c8"], +Cell[101003, 2678, 221, 6, 52, "Output",ExpressionUUID->"fdecf7ce-a690-4d2a-90bb-ba63fe9a6577"] }, Open ]], Cell[CellGroupData[{ -Cell[103707, 2741, 827, 18, 73, "Input",ExpressionUUID->"2dc0fa63-a868-4783-87f2-49c1dcb40d82"], -Cell[104537, 2761, 587, 15, 53, "Output",ExpressionUUID->"2488aad7-c0fe-4f3c-b9a0-6bceaeb7422d"], -Cell[105127, 2778, 449, 7, 34, "Output",ExpressionUUID->"7d97b2e4-726d-4e74-9996-18de0eb3b62a"] +Cell[101261, 2689, 827, 18, 73, "Input",ExpressionUUID->"2dc0fa63-a868-4783-87f2-49c1dcb40d82"], +Cell[102091, 2709, 587, 15, 53, "Output",ExpressionUUID->"2488aad7-c0fe-4f3c-b9a0-6bceaeb7422d"], +Cell[102681, 2726, 449, 7, 34, "Output",ExpressionUUID->"7d97b2e4-726d-4e74-9996-18de0eb3b62a"] }, Open ]], Cell[CellGroupData[{ -Cell[105613, 2790, 1577, 41, 159, "Input",ExpressionUUID->"24c1c053-80c3-4a2c-88a6-6d4c95ef0728"], -Cell[107193, 2833, 861, 22, 60, "Output",ExpressionUUID->"04df113e-12b6-4d82-83cc-fdc54fb34f30"] +Cell[103167, 2738, 1577, 41, 159, "Input",ExpressionUUID->"24c1c053-80c3-4a2c-88a6-6d4c95ef0728"], +Cell[104747, 2781, 861, 22, 60, "Output",ExpressionUUID->"04df113e-12b6-4d82-83cc-fdc54fb34f30"] }, Open ]], Cell[CellGroupData[{ -Cell[108091, 2860, 1452, 43, 115, "Input",ExpressionUUID->"f888d31a-1a5d-4b9f-8b3d-e8a2fbe1fedd"], -Cell[109546, 2905, 1917, 54, 102, "Output",ExpressionUUID->"526ec543-ec8c-4a9d-a247-0396d8fb630c"] +Cell[105645, 2808, 1452, 43, 115, "Input",ExpressionUUID->"f888d31a-1a5d-4b9f-8b3d-e8a2fbe1fedd"], +Cell[107100, 2853, 1917, 54, 102, "Output",ExpressionUUID->"526ec543-ec8c-4a9d-a247-0396d8fb630c"] }, Open ]], Cell[CellGroupData[{ -Cell[111500, 2964, 1878, 53, 73, "Input",ExpressionUUID->"c27c1a53-75cf-42e0-b674-3a9ffb6a0dc3"], -Cell[113381, 3019, 583, 13, 54, "Output",ExpressionUUID->"bf93046e-0f49-4485-8b45-38c284e26597"], -Cell[113967, 3034, 1114, 26, 57, "Output",ExpressionUUID->"47c327cd-74ba-487d-a735-9271fe465116"] +Cell[109054, 2912, 1878, 53, 73, "Input",ExpressionUUID->"c27c1a53-75cf-42e0-b674-3a9ffb6a0dc3"], +Cell[110935, 2967, 583, 13, 54, "Output",ExpressionUUID->"bf93046e-0f49-4485-8b45-38c284e26597"], +Cell[111521, 2982, 1114, 26, 57, "Output",ExpressionUUID->"47c327cd-74ba-487d-a735-9271fe465116"] }, Open ]], Cell[CellGroupData[{ -Cell[115118, 3065, 1625, 44, 73, "Input",ExpressionUUID->"8ea1a1c1-e3f2-4db8-ab7f-3f808af06c72"], -Cell[116746, 3111, 629, 12, 34, "Output",ExpressionUUID->"4c0b888b-aea4-4c4d-8da6-1ffc5f77f43f"] +Cell[112672, 3013, 1625, 44, 73, "Input",ExpressionUUID->"8ea1a1c1-e3f2-4db8-ab7f-3f808af06c72"], +Cell[114300, 3059, 629, 12, 34, "Output",ExpressionUUID->"4c0b888b-aea4-4c4d-8da6-1ffc5f77f43f"] }, Open ]], Cell[CellGroupData[{ -Cell[117412, 3128, 849, 24, 52, "Input",ExpressionUUID->"d9b81e1b-a620-4b83-b247-576167e171a3"], -Cell[118264, 3154, 42511, 1073, 2030, "Output",ExpressionUUID->"4e0cc09a-c5bc-4f3f-8873-5f55f695a4a2"] +Cell[114966, 3076, 849, 24, 52, "Input",ExpressionUUID->"d9b81e1b-a620-4b83-b247-576167e171a3"], +Cell[115818, 3102, 42511, 1073, 2030, "Output",ExpressionUUID->"4e0cc09a-c5bc-4f3f-8873-5f55f695a4a2"] }, Open ]], Cell[CellGroupData[{ -Cell[160812, 4232, 1913, 54, 73, "Input",ExpressionUUID->"a1407b25-8b1c-4678-950e-2905d2867987"], -Cell[162728, 4288, 578, 13, 54, "Output",ExpressionUUID->"950962bc-7fcf-4070-a72d-ac21f7ef17d6"], -Cell[163309, 4303, 2215, 59, 122, "Output",ExpressionUUID->"026f96c6-8e25-4ecd-9ee0-d8049c25b7d0"], -Cell[165527, 4364, 1041, 27, 57, "Output",ExpressionUUID->"9522ce7b-ab7e-40d1-aa77-77ef84ab9939"] +Cell[158366, 4180, 1913, 54, 73, "Input",ExpressionUUID->"a1407b25-8b1c-4678-950e-2905d2867987"], +Cell[160282, 4236, 578, 13, 54, "Output",ExpressionUUID->"950962bc-7fcf-4070-a72d-ac21f7ef17d6"], +Cell[160863, 4251, 2215, 59, 122, "Output",ExpressionUUID->"026f96c6-8e25-4ecd-9ee0-d8049c25b7d0"], +Cell[163081, 4312, 1041, 27, 57, "Output",ExpressionUUID->"9522ce7b-ab7e-40d1-aa77-77ef84ab9939"] }, Open ]], Cell[CellGroupData[{ -Cell[166605, 4396, 931, 24, 30, "Input",ExpressionUUID->"fdf27493-61a0-48a1-a2cd-121356de341c"], -Cell[167539, 4422, 703, 20, 58, "Output",ExpressionUUID->"3f4ab7ce-d531-44d1-a76c-b80cf9707191"] +Cell[164159, 4344, 931, 24, 30, "Input",ExpressionUUID->"fdf27493-61a0-48a1-a2cd-121356de341c"], +Cell[165093, 4370, 703, 20, 58, "Output",ExpressionUUID->"3f4ab7ce-d531-44d1-a76c-b80cf9707191"] }, Open ]], Cell[CellGroupData[{ -Cell[168279, 4447, 807, 21, 73, "Input",ExpressionUUID->"60875d52-63bd-466d-8436-f5d9243d2f74"], -Cell[169089, 4470, 435, 6, 56, "Output",ExpressionUUID->"40870306-99ce-4ee2-a3cc-3bc2d4831823"] +Cell[165833, 4395, 807, 21, 73, "Input",ExpressionUUID->"60875d52-63bd-466d-8436-f5d9243d2f74"], +Cell[166643, 4418, 435, 6, 56, "Output",ExpressionUUID->"40870306-99ce-4ee2-a3cc-3bc2d4831823"] }, Open ]], Cell[CellGroupData[{ -Cell[169561, 4481, 1018, 30, 30, "Input",ExpressionUUID->"74470ae4-6515-4c1e-a697-531c9c82f2cd"], -Cell[170582, 4513, 2132, 58, 102, "Output",ExpressionUUID->"edbcb1ef-7497-4e27-9041-4279244192b8"] +Cell[167115, 4429, 1018, 30, 30, "Input",ExpressionUUID->"74470ae4-6515-4c1e-a697-531c9c82f2cd"], +Cell[168136, 4461, 2132, 58, 102, "Output",ExpressionUUID->"edbcb1ef-7497-4e27-9041-4279244192b8"] }, Open ]], Cell[CellGroupData[{ -Cell[172751, 4576, 918, 28, 30, "Input",ExpressionUUID->"aab49c1b-186c-4921-be5a-411197e871d9"], -Cell[173672, 4606, 1989, 56, 102, "Output",ExpressionUUID->"3a2a36bc-9448-4c8b-96da-12a65fadf973"] +Cell[170305, 4524, 918, 28, 30, "Input",ExpressionUUID->"aab49c1b-186c-4921-be5a-411197e871d9"], +Cell[171226, 4554, 1989, 56, 102, "Output",ExpressionUUID->"3a2a36bc-9448-4c8b-96da-12a65fadf973"] }, Open ]], -Cell[175676, 4665, 1829, 45, 115, "Input",ExpressionUUID->"1ae1d73d-3872-4467-afe0-2a4285eaa8a1"], +Cell[173230, 4613, 1829, 45, 115, "Input",ExpressionUUID->"1ae1d73d-3872-4467-afe0-2a4285eaa8a1"], Cell[CellGroupData[{ -Cell[177530, 4714, 840, 21, 52, "Input",ExpressionUUID->"eeb6fb04-66e9-497d-86d2-19d153d75bad"], -Cell[178373, 4737, 872, 23, 34, "Output",ExpressionUUID->"e1f9b46a-6ed1-410d-8b94-f5164359d144"] +Cell[175084, 4662, 840, 21, 52, "Input",ExpressionUUID->"eeb6fb04-66e9-497d-86d2-19d153d75bad"], +Cell[175927, 4685, 872, 23, 34, "Output",ExpressionUUID->"e1f9b46a-6ed1-410d-8b94-f5164359d144"] }, Open ]], Cell[CellGroupData[{ -Cell[179282, 4765, 1455, 40, 159, "Input",ExpressionUUID->"ce7e4dd2-380c-4e59-9288-6575057b94dc"], -Cell[180740, 4807, 681, 18, 60, "Output",ExpressionUUID->"8f58062b-b931-42bd-8427-acf8a5a4255c"] +Cell[176836, 4713, 1455, 40, 159, "Input",ExpressionUUID->"ce7e4dd2-380c-4e59-9288-6575057b94dc"], +Cell[178294, 4755, 681, 18, 60, "Output",ExpressionUUID->"8f58062b-b931-42bd-8427-acf8a5a4255c"] }, Open ]], Cell[CellGroupData[{ -Cell[181458, 4830, 1029, 31, 52, "Input",ExpressionUUID->"431bc900-2004-40c2-ab81-a63aed4ff595"], -Cell[182490, 4863, 2000, 56, 102, "Output",ExpressionUUID->"6dd3b1e1-ea1e-4c19-ae90-592d85bc526a"] +Cell[179012, 4778, 1029, 31, 52, "Input",ExpressionUUID->"431bc900-2004-40c2-ab81-a63aed4ff595"], +Cell[180044, 4811, 2000, 56, 102, "Output",ExpressionUUID->"6dd3b1e1-ea1e-4c19-ae90-592d85bc526a"] }, Open ]], Cell[CellGroupData[{ -Cell[184527, 4924, 406, 10, 52, "Input",ExpressionUUID->"1e1d00c8-16e2-4441-8e2e-9d37bca8d931"], -Cell[184936, 4936, 471, 8, 77, "Output",ExpressionUUID->"e87b983b-e7da-490e-a23b-caa2c7ac4a74"] +Cell[182081, 4872, 406, 10, 52, "Input",ExpressionUUID->"1e1d00c8-16e2-4441-8e2e-9d37bca8d931"], +Cell[182490, 4884, 471, 8, 77, "Output",ExpressionUUID->"e87b983b-e7da-490e-a23b-caa2c7ac4a74"] }, Open ]], Cell[CellGroupData[{ -Cell[185444, 4949, 371, 11, 51, "Input",ExpressionUUID->"9a1cb538-fbe9-4b4c-b646-ed6c1ec0bb5d"], -Cell[185818, 4962, 1353, 44, 57, "Output",ExpressionUUID->"d3910532-eaa3-44f1-90b5-ed903549d017"] +Cell[182998, 4897, 371, 11, 51, "Input",ExpressionUUID->"9a1cb538-fbe9-4b4c-b646-ed6c1ec0bb5d"], +Cell[183372, 4910, 1353, 44, 57, "Output",ExpressionUUID->"d3910532-eaa3-44f1-90b5-ed903549d017"] }, Open ]] } ] diff --git a/notebooks/ruckig-step2-uddu.nb b/notebooks/ruckig-step2-uddu.nb index 025cf502..c63d27bd 100644 --- a/notebooks/ruckig-step2-uddu.nb +++ b/notebooks/ruckig-step2-uddu.nb @@ -10,10 +10,10 @@ NotebookFileLineBreakTest NotebookFileLineBreakTest NotebookDataPosition[ 158, 7] -NotebookDataLength[ 116348, 3046] -NotebookOptionsPosition[ 109947, 2934] -NotebookOutlinePosition[ 110372, 2951] -CellTagsIndexPosition[ 110329, 2948] +NotebookDataLength[ 120722, 3142] +NotebookOptionsPosition[ 114438, 3033] +NotebookOutlinePosition[ 114867, 3050] +CellTagsIndexPosition[ 114824, 3047] WindowFrame->Normal*) (* Beginning of Notebook Content *) @@ -251,9 +251,8 @@ Cell[BoxData[{ 3.817577983155306*^9, 3.817610819742194*^9, 3.819359229321691*^9, 3.819359611425458*^9, {3.819359641697466*^9, 3.8193596426416473`*^9}, { 3.820569527654592*^9, 3.8205695304639874`*^9}}, - CellLabel->"In[32]:=",ExpressionUUID->"9382707b-2876-460d-b0fb-ca70e05b1e9a"], - -Cell[CellGroupData[{ + CellLabel-> + "In[166]:=",ExpressionUUID->"9382707b-2876-460d-b0fb-ca70e05b1e9a"], Cell[BoxData[ RowBox[{ @@ -269,24 +268,7 @@ Cell[BoxData[ RowBox[{"p7", "\[Equal]", "pf"}], ",", RowBox[{"a3", "\[Equal]", "0"}], ",", RowBox[{"a1", "\[Equal]", "aMax"}], ",", - RowBox[{"a5", "\[Equal]", - RowBox[{"-", "aMax"}]}], ",", - RowBox[{"v3", "\[Equal]", "vMax"}], ",", - RowBox[{"tAll", "\[Equal]", "tf"}]}], "}"}], ",", - RowBox[{"Join", "[", - RowBox[{"tVars", ",", - RowBox[{"{", "vMax", "}"}]}], "]"}]}], "]"}], ",", - "\[IndentingNewLine]", - RowBox[{"Solve", "[", - RowBox[{ - RowBox[{"{", - RowBox[{ - RowBox[{"a7", "\[Equal]", "af"}], ",", - RowBox[{"v7", "\[Equal]", "vf"}], ",", - RowBox[{"p7", "\[Equal]", "pf"}], ",", - RowBox[{"a3", "\[Equal]", "0"}], ",", - RowBox[{"a1", "\[Equal]", "aMax"}], ",", - RowBox[{"a5", "\[Equal]", "aMax"}], ",", + RowBox[{"a5", "\[Equal]", "aMin"}], ",", RowBox[{"v3", "\[Equal]", "vMax"}], ",", RowBox[{"tAll", "\[Equal]", "tf"}]}], "}"}], ",", RowBox[{"Join", "[", @@ -302,8 +284,7 @@ Cell[BoxData[ RowBox[{"p7", "\[Equal]", "pf"}], ",", RowBox[{"a3", "\[Equal]", "0"}], ",", RowBox[{"a1", "\[Equal]", "aMax"}], ",", - RowBox[{"a5", "\[Equal]", - RowBox[{"-", "aMax"}]}], ",", + RowBox[{"a5", "\[Equal]", "aMin"}], ",", RowBox[{"t4", "\[Equal]", "0"}], ",", RowBox[{"tAll", "\[Equal]", "tf"}]}], "}"}], ",", RowBox[{"Join", "[", @@ -362,7 +343,7 @@ Cell[BoxData[ RowBox[{ RowBox[{"a0", "\[Rule]", "0"}], ",", RowBox[{"af", "\[Rule]", "0"}]}], "}"}]}], ")"}], "\[Equal]", - RowBox[{"-", "aMax"}]}], ",", + "aMin"}], ",", RowBox[{"t4", "\[Equal]", "0"}], ",", RowBox[{"tAll", "\[Equal]", "tf"}]}], "}"}], ",", RowBox[{"Join", "[", @@ -378,26 +359,7 @@ Cell[BoxData[ RowBox[{"p7", "\[Equal]", "pf"}], ",", RowBox[{"a3", "\[Equal]", "0"}], ",", RowBox[{"t2", "\[Equal]", "0"}], ",", - RowBox[{"a5", "\[Equal]", - RowBox[{"-", "aMax"}]}], ",", - RowBox[{"v3", "\[Equal]", "vMax"}], ",", - RowBox[{"tAll", "\[Equal]", "tf"}]}], "}"}], ",", - RowBox[{"Join", "[", - RowBox[{"tVars", ",", - RowBox[{"{", "vMax", "}"}]}], "]"}], ",", - RowBox[{"Cubics", "\[Rule]", "False"}], ",", - RowBox[{"Quartics", "\[Rule]", "False"}]}], "]"}], ",", - "\[IndentingNewLine]", - RowBox[{"Solve", "[", - RowBox[{ - RowBox[{"{", - RowBox[{ - RowBox[{"a7", "\[Equal]", "af"}], ",", - RowBox[{"v7", "\[Equal]", "vf"}], ",", - RowBox[{"p7", "\[Equal]", "pf"}], ",", - RowBox[{"a3", "\[Equal]", "0"}], ",", - RowBox[{"t2", "\[Equal]", "0"}], ",", - RowBox[{"a5", "\[Equal]", "aMax"}], ",", + RowBox[{"a5", "\[Equal]", "aMin"}], ",", RowBox[{"v3", "\[Equal]", "vMax"}], ",", RowBox[{"tAll", "\[Equal]", "tf"}]}], "}"}], ",", RowBox[{"Join", "[", @@ -570,40 +532,33 @@ Cell[BoxData[ 3.817648004690806*^9, 3.817648004812105*^9}, {3.817648049574768*^9, 3.8176480544861193`*^9}, {3.817648706998025*^9, 3.817648726629472*^9}, { 3.8176487661502733`*^9, 3.81764877682362*^9}, {3.817648986736754*^9, - 3.817649005091858*^9}, 3.817649060563797*^9}, + 3.817649005091858*^9}, 3.817649060563797*^9, {3.821872831864657*^9, + 3.821872856937278*^9}, {3.821872894493676*^9, 3.821872899485652*^9}, { + 3.8218740033715467`*^9, 3.821874004354208*^9}}, CellLabel-> - "In[129]:=",ExpressionUUID->"93a49805-7b39-4e71-a5ce-78f2b77ae9c0"], - -Cell[BoxData["$Aborted"], "Output", - CellChangeTimes->{3.819359328459915*^9, 3.819359403231781*^9, - 3.8195631130890636`*^9}, - CellLabel-> - "Out[129]=",ExpressionUUID->"7022ff95-9f21-46fd-84d4-9abc7a83743a"] -}, Open ]], + "In[240]:=",ExpressionUUID->"93a49805-7b39-4e71-a5ce-78f2b77ae9c0"], Cell["\<\ Information -- (1) Case 1a: Acc0_Acc1_Vel (UDDU) Solution 1 -- (2) Case 1b: Acc0_Acc1_Vel (UDUD) Solution 1 -- (3) Case 2a: Acc0_Acc1 Solution 1 -- (4) Case 2b: Acc0_Acc1 (a0=0, af=0) Solution 1 -- (5) Case 3a: Acc1_Vel (UDDU), Root t1 -- (6) Case 3b: Acc1_Vel (UDUD), Root t1 -- (7) Case 4: Acc0_Vel (UDDU), Root t5 -- (8) Case 5: Vel, Root t1 -- (9) Case 8a: None, Root resultT[[9,1,1,2,4,25,5]] -- (10) Case 8b: None a0 ==0, af == 0, v0==0, vf == 0, Solution 1/2 -- (11) Case 8c: None a0 == 0, af == 0, Solution 1\ +- (1) Case 1a: Acc0_Acc1_Vel Solution 1 +- (2) Case 2a: Acc0_Acc1 Solution 1 +- (3) Case 2b: Acc0_Acc1 (a0=0, af=0) Solution 1 +- (4) Case 3a: Acc1_Vel, Root t1 +- (5) Case 4: Acc0_Vel, Root t5 +- (6) Case 5: Vel, Root t1 +- (7) Case 8a: None, Root resultT[[9,1,1,2,4,25,5]] +- (8) Case 8b: None a0 ==0, af == 0, v0==0, vf == 0, Solution 1/2 +- (9) Case 8c: None a0 == 0, af == 0, Solution 1\ \>", "Text", CellChangeTimes->CompressedData[" -1:eJxTTMoPSmViYGAQB2IQbTShJSah4I3jq65TYFrv6UmPiUB6UbK9J4je/cE+ +1:eJxTTMoPSmViYGCQAmIQbTShJSah4I3jq65TYFrv6UmPiUB6UbK9J4je/cE+ DESf++cIplM2+UpNAtKX/vuDaT6FGO1pQHrCut1g2uMAx7oZQNrg5XkwvWmu 5gkQLZ+mDaZD79R6OhYC6fLnESD6Nf/Rzx5A+n6OLZsnkF5T0BAPok36NoDp WfknZ8cA6UOxH8H0gQsRPIlA2u1OFJgucLUVzgTRMxeIguhpceWSIPpe4Qcp EF2xvSsZRLPpbQHTkk/5c0F0x7y8PBDttuJlLYjmKX8Fpp+qz3k6FWSvbegz EL2/cP5/EP33zgcw/a7lveI0kLz9FzB92kY1HESfMDoCpoNS7uSB6PWb7oHp -lluqpSA6roy9DEQDAEJVvs4= - +lluqpSA6roy9DES7WxXdXFL5xlHJqusWiD70etcDEP1Hx/ohiJ50IyRnKZA+ +pWGQC6IBnZvZXw== "],ExpressionUUID->"0616ae41-3a02-473a-878f-96412e2fc8c5"], Cell["\<\ @@ -622,7 +577,7 @@ Cell[BoxData[{ RowBox[{"Simplify", "[", RowBox[{"resultT", "[", RowBox[{"[", - RowBox[{"8", ",", "1", ",", "1", ",", "2"}], "]"}], "]"}], + RowBox[{"1", ",", "1", ",", "2", ",", "2"}], "]"}], "]"}], "]"}]}], "\[IndentingNewLine]", RowBox[{ RowBox[{"ToString", "[", @@ -647,11 +602,154 @@ Cell[BoxData[{ 3.817557010454414*^9}, {3.817557048801483*^9, 3.817557153609576*^9}, { 3.8176488340137253`*^9, 3.817648841328828*^9}, {3.8176488718669558`*^9, 3.8176489145136023`*^9}, {3.81764898204137*^9, 3.817648992201417*^9}, { - 3.817649029172593*^9, 3.817649029234479*^9}, - 3.817794746450993*^9},ExpressionUUID->"f08feb2e-6079-473b-b866-\ -92670dea0a56"], + 3.817649029172593*^9, 3.817649029234479*^9}, 3.817794746450993*^9, { + 3.821872970462631*^9, 3.82187310650366*^9}}, + CellLabel-> + "In[237]:=",ExpressionUUID->"f08feb2e-6079-473b-b866-92670dea0a56"], -Cell[BoxData["$Aborted"], "Output", +Cell[BoxData[ + RowBox[{"-", + RowBox[{ + FractionBox["1", + RowBox[{"6", " ", "aMax", " ", + RowBox[{"(", + RowBox[{"aMax", "-", "aMin"}], ")"}], " ", + SuperscriptBox["jMax", "2"]}]], + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "3"}], " ", + SuperscriptBox["a0", "2"], " ", "aMax", " ", "jMax"}], "+", + RowBox[{"3", " ", + SuperscriptBox["af", "2"], " ", "aMax", " ", "jMax"}], "+", + RowBox[{"6", " ", + SuperscriptBox["aMax", "3"], " ", "jMax"}], "+", + RowBox[{"6", " ", "a0", " ", "aMax", " ", "aMin", " ", "jMax"}], "-", + RowBox[{"6", " ", "af", " ", "aMax", " ", "aMin", " ", "jMax"}], "-", + RowBox[{"9", " ", + SuperscriptBox["aMax", "2"], " ", "aMin", " ", "jMax"}], "+", + RowBox[{"3", " ", "aMax", " ", + SuperscriptBox["aMin", "2"], " ", "jMax"}], "+", + RowBox[{"6", " ", "aMax", " ", "aMin", " ", + SuperscriptBox["jMax", "2"], " ", "tf"}], "+", + RowBox[{"6", " ", "aMax", " ", + SuperscriptBox["jMax", "2"], " ", "v0"}], "-", + RowBox[{"6", " ", "aMax", " ", + SuperscriptBox["jMax", "2"], " ", "vf"}], "+", + RowBox[{ + SqrtBox["3"], " ", + RowBox[{"\[Sqrt]", + RowBox[{"(", + RowBox[{"aMax", " ", "aMin", " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"3", " ", + SuperscriptBox["a0", "4"]}], "+", + RowBox[{"3", " ", + SuperscriptBox["af", "4"]}], "-", + RowBox[{"4", " ", + SuperscriptBox["a0", "3"], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "aMax"}], "+", "aMin"}], ")"}]}], "-", + RowBox[{"4", " ", + SuperscriptBox["af", "3"], " ", + RowBox[{"(", + RowBox[{"aMax", "+", + RowBox[{"2", " ", "aMin"}]}], ")"}]}], "-", + RowBox[{"6", " ", + SuperscriptBox["a0", "2"], " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["af", "2"], "-", + SuperscriptBox["aMax", "2"], "-", + RowBox[{"2", " ", "af", " ", "aMin"}], "-", + RowBox[{"2", " ", "aMax", " ", "aMin"}], "+", + SuperscriptBox["aMin", "2"], "+", + RowBox[{"2", " ", "aMin", " ", "jMax", " ", "tf"}], "+", + RowBox[{"2", " ", "jMax", " ", "v0"}], "-", + RowBox[{"2", " ", "jMax", " ", "vf"}]}], ")"}]}], "+", + RowBox[{"12", " ", "a0", " ", "aMax", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["af", "2"], "-", + RowBox[{"2", " ", "af", " ", "aMin"}], "-", + RowBox[{"aMax", " ", "aMin"}], "+", + SuperscriptBox["aMin", "2"], "+", + RowBox[{"2", " ", "aMin", " ", "jMax", " ", "tf"}], "+", + RowBox[{"2", " ", "jMax", " ", "v0"}], "-", + RowBox[{"2", " ", "jMax", " ", "vf"}]}], ")"}]}], "-", + RowBox[{"6", " ", + SuperscriptBox["af", "2"], " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["aMax", "2"], "-", + SuperscriptBox["aMin", "2"], "-", + RowBox[{"2", " ", "aMax", " ", + RowBox[{"(", + RowBox[{"aMin", "+", + RowBox[{"jMax", " ", "tf"}]}], ")"}]}], "-", + RowBox[{"2", " ", "jMax", " ", "v0"}], "+", + RowBox[{"2", " ", "jMax", " ", "vf"}]}], ")"}]}], "+", + RowBox[{"12", " ", "af", " ", "aMin", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["aMax", "2"], "-", + RowBox[{"aMax", " ", + RowBox[{"(", + RowBox[{"aMin", "+", + RowBox[{"2", " ", "jMax", " ", "tf"}]}], ")"}]}], "+", + RowBox[{"2", " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "v0"}], "+", "vf"}], ")"}]}]}], ")"}]}], "+", + RowBox[{"3", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + SuperscriptBox["aMax", "3"], " ", "aMin"}], "-", + RowBox[{"2", " ", + SuperscriptBox["aMax", "2"], " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["aMin", "2"], "+", + RowBox[{"2", " ", "aMin", " ", "jMax", " ", "tf"}], "+", + RowBox[{"2", " ", "jMax", " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}]}]}], ")"}]}], "+", + RowBox[{"4", " ", "jMax", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", "aMin", " ", "jMax", " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf", "+", + RowBox[{"tf", " ", "v0"}]}], ")"}]}], "+", + RowBox[{ + SuperscriptBox["aMin", "2"], " ", + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}]}], "+", + RowBox[{"jMax", " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{"v0", "-", "vf"}], ")"}], "2"]}]}], ")"}]}], "+", + + RowBox[{"aMax", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["aMin", "3"], "+", + RowBox[{"4", " ", + SuperscriptBox["aMin", "2"], " ", "jMax", " ", "tf"}], "+", + + RowBox[{"4", " ", "aMin", " ", + SuperscriptBox["jMax", "2"], " ", + SuperscriptBox["tf", "2"]}], "-", + RowBox[{"8", " ", + SuperscriptBox["jMax", "2"], " ", + RowBox[{"(", + RowBox[{"p0", "-", "pf", "+", + RowBox[{"tf", " ", "vf"}]}], ")"}]}]}], ")"}]}]}], + ")"}]}]}], ")"}]}], ")"}]}]}]}], ")"}]}]}]], "Output", CellChangeTimes->{{3.817013038100574*^9, 3.8170130952655783`*^9}, { 3.817013843881184*^9, 3.817013913172606*^9}, 3.817013966856229*^9, { 3.8171141944209747`*^9, 3.817114247671588*^9}, 3.8171142928777733`*^9, { @@ -668,9 +766,10 @@ Cell[BoxData["$Aborted"], "Output", 3.817556857265253*^9, {3.817556888055502*^9, 3.817556915762059*^9}, { 3.817557012891211*^9, 3.817557155732799*^9}, 3.817648842091989*^9, { 3.817648872270245*^9, 3.81764891512903*^9}, {3.817648983088738*^9, - 3.817648993522546*^9}, {3.8176490273098507`*^9, 3.817649052425692*^9}}, + 3.817648993522546*^9}, {3.8176490273098507`*^9, 3.817649052425692*^9}, { + 3.821872971292325*^9, 3.821873106743492*^9}}, CellLabel-> - "Out[410]=",ExpressionUUID->"6ba93a6f-1693-4cd0-bd08-3827239d3713"] + "Out[237]=",ExpressionUUID->"b656b933-26e5-453f-92be-0fa1092c2558"] }, Open ]], Cell["Print Roots", "Text", @@ -2933,9 +3032,9 @@ Cell[BoxData[ CellLabel->"In[82]:=",ExpressionUUID->"60b39820-7bf7-4bd1-88ef-79b5667de827"] }, WindowSize->{979, 784}, -WindowMargins->{{Automatic, 30}, {-247, Automatic}}, +WindowMargins->{{Automatic, -51}, {-60, Automatic}}, Magnification:>0.9 Inherited, -FrontEndVersion->"12.1 for Mac OS X x86 (64-bit) (June 19, 2020)", +FrontEndVersion->"12.2 for Mac OS X x86 (64-bit) (December 12, 2020)", StyleDefinitions->"Default.nb", ExpressionUUID->"f330ac05-689b-44a4-abe8-a4fef195fff9" ] @@ -2951,103 +3050,100 @@ CellTagsIndex->{} (*NotebookFileOutline Notebook[{ Cell[558, 20, 230, 4, 31, "Text",ExpressionUUID->"039b787c-64fd-4dfb-8011-578c383c402e"], -Cell[791, 26, 7516, 227, 765, "Input",ExpressionUUID->"9382707b-2876-460d-b0fb-ca70e05b1e9a"], -Cell[CellGroupData[{ -Cell[8332, 257, 12691, 317, 408, "Input",ExpressionUUID->"93a49805-7b39-4e71-a5ce-78f2b77ae9c0"], -Cell[21026, 576, 209, 4, 31, "Output",ExpressionUUID->"7022ff95-9f21-46fd-84d4-9abc7a83743a"] -}, Open ]], -Cell[21250, 583, 1021, 23, 259, "Text",ExpressionUUID->"0616ae41-3a02-473a-878f-96412e2fc8c5"], -Cell[22274, 608, 284, 7, 52, "Text",ExpressionUUID->"407acd61-eed1-4280-b150-0da33c32906a"], +Cell[791, 26, 7520, 228, 765, "Input",ExpressionUUID->"9382707b-2876-460d-b0fb-ca70e05b1e9a"], +Cell[8314, 256, 11363, 282, 351, "Input",ExpressionUUID->"93a49805-7b39-4e71-a5ce-78f2b77ae9c0"], +Cell[19680, 540, 961, 21, 218, "Text",ExpressionUUID->"0616ae41-3a02-473a-878f-96412e2fc8c5"], +Cell[20644, 563, 284, 7, 52, "Text",ExpressionUUID->"407acd61-eed1-4280-b150-0da33c32906a"], Cell[CellGroupData[{ -Cell[22583, 619, 1927, 32, 66, "Input",ExpressionUUID->"f08feb2e-6079-473b-b866-92670dea0a56"], -Cell[24513, 653, 1359, 19, 31, "Output",ExpressionUUID->"6ba93a6f-1693-4cd0-bd08-3827239d3713"] +Cell[20953, 574, 1998, 33, 66, "Input",ExpressionUUID->"f08feb2e-6079-473b-b866-92670dea0a56"], +Cell[22954, 609, 7409, 162, 215, "Output",ExpressionUUID->"b656b933-26e5-453f-92be-0fa1092c2558"] }, Open ]], -Cell[25887, 675, 247, 4, 31, "Text",ExpressionUUID->"f1684154-b22e-4827-98da-4e4dad150835"], +Cell[30378, 774, 247, 4, 31, "Text",ExpressionUUID->"f1684154-b22e-4827-98da-4e4dad150835"], Cell[CellGroupData[{ -Cell[26159, 683, 2231, 55, 163, "Input",ExpressionUUID->"217d7885-1018-483e-8952-08082feb5695"], -Cell[28393, 740, 4547, 121, 141, "Output",ExpressionUUID->"0add838c-dd53-4b4a-9a83-667a65b6c2dc"] +Cell[30650, 782, 2231, 55, 163, "Input",ExpressionUUID->"217d7885-1018-483e-8952-08082feb5695"], +Cell[32884, 839, 4547, 121, 141, "Output",ExpressionUUID->"0add838c-dd53-4b4a-9a83-667a65b6c2dc"] }, Open ]], Cell[CellGroupData[{ -Cell[32977, 866, 1377, 28, 85, "Input",ExpressionUUID->"58c30e25-83c4-4d05-ad21-e88fcb7f4181"], -Cell[34357, 896, 862, 14, 49, "Output",ExpressionUUID->"28ef38fa-fff6-4035-92dc-49a30c5e7739"] +Cell[37468, 965, 1377, 28, 85, "Input",ExpressionUUID->"58c30e25-83c4-4d05-ad21-e88fcb7f4181"], +Cell[38848, 995, 862, 14, 49, "Output",ExpressionUUID->"28ef38fa-fff6-4035-92dc-49a30c5e7739"] }, Open ]], -Cell[35234, 913, 229, 4, 31, "Text",ExpressionUUID->"5c00d19c-efbb-475f-a4a4-adc1877059e7"], -Cell[35466, 919, 5870, 131, 275, "Input",ExpressionUUID->"013ba19d-162c-4e14-9e6b-43bcf125ceef"], -Cell[41339, 1052, 869, 20, 218, "Text",ExpressionUUID->"ccf44efd-e09a-4bbb-a696-b4c90f7a95eb"], +Cell[39725, 1012, 229, 4, 31, "Text",ExpressionUUID->"5c00d19c-efbb-475f-a4a4-adc1877059e7"], +Cell[39957, 1018, 5870, 131, 275, "Input",ExpressionUUID->"013ba19d-162c-4e14-9e6b-43bcf125ceef"], +Cell[45830, 1151, 869, 20, 218, "Text",ExpressionUUID->"ccf44efd-e09a-4bbb-a696-b4c90f7a95eb"], Cell[CellGroupData[{ -Cell[42233, 1076, 838, 19, 66, "Input",ExpressionUUID->"b4fd3303-7c30-4af8-94b1-4505577eaf40"], -Cell[43074, 1097, 5908, 138, 161, "Output",ExpressionUUID->"e653eb3d-f560-4f2c-9646-ccf546b5417c"] +Cell[46724, 1175, 838, 19, 66, "Input",ExpressionUUID->"b4fd3303-7c30-4af8-94b1-4505577eaf40"], +Cell[47565, 1196, 5908, 138, 161, "Output",ExpressionUUID->"e653eb3d-f560-4f2c-9646-ccf546b5417c"] }, Open ]], -Cell[48997, 1238, 204, 4, 31, "Text",ExpressionUUID->"6e1bb727-a1dc-4c24-af7f-abfa0f3b9409"], +Cell[53488, 1337, 204, 4, 31, "Text",ExpressionUUID->"6e1bb727-a1dc-4c24-af7f-abfa0f3b9409"], Cell[CellGroupData[{ -Cell[49226, 1246, 1898, 46, 144, "Input",ExpressionUUID->"b4d0a35e-2a8e-482f-b0a5-593ccb90a6dd"], -Cell[51127, 1294, 7831, 209, 189, "Output",ExpressionUUID->"c74db8a1-1f65-4bfc-962e-fffe0d610493"] +Cell[53717, 1345, 1898, 46, 144, "Input",ExpressionUUID->"b4d0a35e-2a8e-482f-b0a5-593ccb90a6dd"], +Cell[55618, 1393, 7831, 209, 189, "Output",ExpressionUUID->"c74db8a1-1f65-4bfc-962e-fffe0d610493"] }, Open ]], Cell[CellGroupData[{ -Cell[58995, 1508, 1044, 23, 85, "Input",ExpressionUUID->"111d8f11-4472-4521-8634-15b568e443ee"], -Cell[60042, 1533, 4861, 138, 123, "Output",ExpressionUUID->"6ac34b7e-fe01-4d94-8ecc-37e88505a8af"] +Cell[63486, 1607, 1044, 23, 85, "Input",ExpressionUUID->"111d8f11-4472-4521-8634-15b568e443ee"], +Cell[64533, 1632, 4861, 138, 123, "Output",ExpressionUUID->"6ac34b7e-fe01-4d94-8ecc-37e88505a8af"] }, Open ]], Cell[CellGroupData[{ -Cell[64940, 1676, 1812, 44, 116, "Input",ExpressionUUID->"0d5d8425-f226-4e6a-91fd-34a5822f73b9"], -Cell[66755, 1722, 2994, 80, 155, "Output",ExpressionUUID->"1dcd2096-117e-4fa7-8a27-82d21901823d"] +Cell[69431, 1775, 1812, 44, 116, "Input",ExpressionUUID->"0d5d8425-f226-4e6a-91fd-34a5822f73b9"], +Cell[71246, 1821, 2994, 80, 155, "Output",ExpressionUUID->"1dcd2096-117e-4fa7-8a27-82d21901823d"] }, Open ]], Cell[CellGroupData[{ -Cell[69786, 1807, 764, 21, 46, "Input",ExpressionUUID->"4f3c940a-b41f-43f3-9aa8-8115c96b6bee"], -Cell[70553, 1830, 2830, 70, 76, "Output",ExpressionUUID->"f8a67d4a-9073-4868-948b-275b14bd35f8"] +Cell[74277, 1906, 764, 21, 46, "Input",ExpressionUUID->"4f3c940a-b41f-43f3-9aa8-8115c96b6bee"], +Cell[75044, 1929, 2830, 70, 76, "Output",ExpressionUUID->"f8a67d4a-9073-4868-948b-275b14bd35f8"] }, Open ]], -Cell[73398, 1903, 472, 14, 42, "Input",ExpressionUUID->"ac4e2179-0486-40d4-a93e-0bd772b55249"], -Cell[73873, 1919, 190, 3, 31, "Text",ExpressionUUID->"56699e4e-5d18-41ce-a54a-bd05638d5d2d"], +Cell[77889, 2002, 472, 14, 42, "Input",ExpressionUUID->"ac4e2179-0486-40d4-a93e-0bd772b55249"], +Cell[78364, 2018, 190, 3, 31, "Text",ExpressionUUID->"56699e4e-5d18-41ce-a54a-bd05638d5d2d"], Cell[CellGroupData[{ -Cell[74088, 1926, 895, 24, 42, "Input",ExpressionUUID->"5e6a25cd-7819-4650-a7d0-08f771745532"], -Cell[74986, 1952, 561, 15, 35, "Output",ExpressionUUID->"3d5e1dff-06d5-4a78-8211-d677f7d95c06"] +Cell[78579, 2025, 895, 24, 42, "Input",ExpressionUUID->"5e6a25cd-7819-4650-a7d0-08f771745532"], +Cell[79477, 2051, 561, 15, 35, "Output",ExpressionUUID->"3d5e1dff-06d5-4a78-8211-d677f7d95c06"] }, Open ]], Cell[CellGroupData[{ -Cell[75584, 1972, 1011, 27, 27, "Input",ExpressionUUID->"02c4da17-fbe6-4543-b195-45be4ca9b841"], -Cell[76598, 2001, 4330, 118, 169, "Output",ExpressionUUID->"17ffb416-f6fc-47ba-8ab0-4b6dd6842181"] +Cell[80075, 2071, 1011, 27, 27, "Input",ExpressionUUID->"02c4da17-fbe6-4543-b195-45be4ca9b841"], +Cell[81089, 2100, 4330, 118, 169, "Output",ExpressionUUID->"17ffb416-f6fc-47ba-8ab0-4b6dd6842181"] }, Open ]], Cell[CellGroupData[{ -Cell[80965, 2124, 619, 15, 66, "Input",ExpressionUUID->"7670aa61-4700-4432-8cc7-2b72034ed478"], -Cell[81587, 2141, 1008, 28, 50, "Output",ExpressionUUID->"f9a3e9af-66df-4068-9321-85fb8bf641e1"] +Cell[85456, 2223, 619, 15, 66, "Input",ExpressionUUID->"7670aa61-4700-4432-8cc7-2b72034ed478"], +Cell[86078, 2240, 1008, 28, 50, "Output",ExpressionUUID->"f9a3e9af-66df-4068-9321-85fb8bf641e1"] }, Open ]], Cell[CellGroupData[{ -Cell[82632, 2174, 608, 17, 46, "Input",ExpressionUUID->"c3eac5a8-2fa9-4c22-9d0b-ae3fc680cf99"], -Cell[83243, 2193, 691, 19, 54, "Output",ExpressionUUID->"4dc6f8bf-3425-4ace-bcc0-7ad3cf515abf"] +Cell[87123, 2273, 608, 17, 46, "Input",ExpressionUUID->"c3eac5a8-2fa9-4c22-9d0b-ae3fc680cf99"], +Cell[87734, 2292, 691, 19, 54, "Output",ExpressionUUID->"4dc6f8bf-3425-4ace-bcc0-7ad3cf515abf"] }, Open ]], -Cell[83949, 2215, 234, 4, 31, "Text",ExpressionUUID->"f8c02129-5c08-4287-9ce4-9d2249b49a47"], +Cell[88440, 2314, 234, 4, 31, "Text",ExpressionUUID->"f8c02129-5c08-4287-9ce4-9d2249b49a47"], Cell[CellGroupData[{ -Cell[84208, 2223, 1930, 48, 106, "Input",ExpressionUUID->"60805ad3-28dc-4b78-ba4e-f0602908471c"], -Cell[86141, 2273, 2173, 53, 92, "Output",ExpressionUUID->"c5369eac-c19b-4faa-8087-85202ad8609e"] +Cell[88699, 2322, 1930, 48, 106, "Input",ExpressionUUID->"60805ad3-28dc-4b78-ba4e-f0602908471c"], +Cell[90632, 2372, 2173, 53, 92, "Output",ExpressionUUID->"c5369eac-c19b-4faa-8087-85202ad8609e"] }, Open ]], Cell[CellGroupData[{ -Cell[88351, 2331, 448, 11, 47, "Input",ExpressionUUID->"1adf51d5-6fe8-4769-923d-21e404dcac21"], -Cell[88802, 2344, 290, 4, 31, "Output",ExpressionUUID->"bf241e25-201f-42dc-ae02-700d684d147e"] +Cell[92842, 2430, 448, 11, 47, "Input",ExpressionUUID->"1adf51d5-6fe8-4769-923d-21e404dcac21"], +Cell[93293, 2443, 290, 4, 31, "Output",ExpressionUUID->"bf241e25-201f-42dc-ae02-700d684d147e"] }, Open ]], Cell[CellGroupData[{ -Cell[89129, 2353, 786, 19, 66, "Input",ExpressionUUID->"921d5f93-0895-485d-9d3c-e459a83a8814"], -Cell[89918, 2374, 5743, 154, 317, "Output",ExpressionUUID->"0b148223-b092-480e-ba11-e425965fccb8"] +Cell[93620, 2452, 786, 19, 66, "Input",ExpressionUUID->"921d5f93-0895-485d-9d3c-e459a83a8814"], +Cell[94409, 2473, 5743, 154, 317, "Output",ExpressionUUID->"0b148223-b092-480e-ba11-e425965fccb8"] }, Open ]], Cell[CellGroupData[{ -Cell[95698, 2533, 569, 15, 66, "Input",ExpressionUUID->"9038a11c-84fb-477d-abca-0d7fed1418f2"], -Cell[96270, 2550, 2231, 65, 159, "Output",ExpressionUUID->"3dfbcaea-61a7-48dc-8700-3600d97979e7"] +Cell[100189, 2632, 569, 15, 66, "Input",ExpressionUUID->"9038a11c-84fb-477d-abca-0d7fed1418f2"], +Cell[100761, 2649, 2231, 65, 159, "Output",ExpressionUUID->"3dfbcaea-61a7-48dc-8700-3600d97979e7"] }, Open ]], Cell[CellGroupData[{ -Cell[98538, 2620, 1432, 39, 144, "Input",ExpressionUUID->"50f38d09-add8-4aaa-80cf-82c2c2960cb5"], -Cell[99973, 2661, 936, 26, 54, "Output",ExpressionUUID->"ba57fae9-f30c-4730-8996-2fb86edb2fca"] +Cell[103029, 2719, 1432, 39, 144, "Input",ExpressionUUID->"50f38d09-add8-4aaa-80cf-82c2c2960cb5"], +Cell[104464, 2760, 936, 26, 54, "Output",ExpressionUUID->"ba57fae9-f30c-4730-8996-2fb86edb2fca"] }, Open ]], Cell[CellGroupData[{ -Cell[100946, 2692, 946, 26, 42, "Input",ExpressionUUID->"ecc98e15-0b26-499a-88a0-118989f052e9"], -Cell[101895, 2720, 1677, 55, 121, "Output",ExpressionUUID->"55efec97-7768-49b4-945b-81d8a50e7a37"] +Cell[105437, 2791, 946, 26, 42, "Input",ExpressionUUID->"ecc98e15-0b26-499a-88a0-118989f052e9"], +Cell[106386, 2819, 1677, 55, 121, "Output",ExpressionUUID->"55efec97-7768-49b4-945b-81d8a50e7a37"] }, Open ]], -Cell[103587, 2778, 152, 3, 27, "Input",ExpressionUUID->"f9662b36-78d8-4212-a221-e86d60821dfb"], +Cell[108078, 2877, 152, 3, 27, "Input",ExpressionUUID->"f9662b36-78d8-4212-a221-e86d60821dfb"], Cell[CellGroupData[{ -Cell[103764, 2785, 1014, 26, 46, "Input",ExpressionUUID->"95b2f07a-c6f4-4b0d-8373-c2aab7bca898"], -Cell[104781, 2813, 1835, 43, 78, "Output",ExpressionUUID->"0580b0f6-fedd-4957-b0cf-802634e3d476"] +Cell[108255, 2884, 1014, 26, 46, "Input",ExpressionUUID->"95b2f07a-c6f4-4b0d-8373-c2aab7bca898"], +Cell[109272, 2912, 1835, 43, 78, "Output",ExpressionUUID->"0580b0f6-fedd-4957-b0cf-802634e3d476"] }, Open ]], Cell[CellGroupData[{ -Cell[106653, 2861, 540, 12, 47, "Input",ExpressionUUID->"7fa61b3c-5739-47ca-8868-6a28a2b93186"], -Cell[107196, 2875, 1600, 22, 241, "Output",ExpressionUUID->"fcdeb4d9-16d1-4d74-875b-ac4da2a98d8f"] +Cell[111144, 2960, 540, 12, 47, "Input",ExpressionUUID->"7fa61b3c-5739-47ca-8868-6a28a2b93186"], +Cell[111687, 2974, 1600, 22, 241, "Output",ExpressionUUID->"fcdeb4d9-16d1-4d74-875b-ac4da2a98d8f"] }, Open ]], -Cell[108811, 2900, 1132, 32, 47, "Input",ExpressionUUID->"60b39820-7bf7-4bd1-88ef-79b5667de827"] +Cell[113302, 2999, 1132, 32, 47, "Input",ExpressionUUID->"60b39820-7bf7-4bd1-88ef-79b5667de827"] } ] *) diff --git a/src/brake.cpp b/src/brake.cpp index c6f31b20..40ac7dda 100644 --- a/src/brake.cpp +++ b/src/brake.cpp @@ -11,7 +11,7 @@ inline double v_at_a_zero(double v0, double a0, double j) { return v0 + (a0 * a0)/(2 * j); } -void Brake::acceleration_brake(double v0, double a0, double vMax, double vMin, double aMax, double jMax, std::array& t_brake, std::array& j_brake) { +void Brake::acceleration_brake(double v0, double a0, double vMax, double vMin, double aMax, double aMin, double jMax, std::array& t_brake, std::array& j_brake) { j_brake[0] = -jMax; const double t_to_a_max = (a0 - aMax) / jMax; @@ -21,7 +21,7 @@ void Brake::acceleration_brake(double v0, double a0, double vMax, double vMin, d const double v_at_a_zero = v_at_t(v0, a0, -jMax, t_to_a_zero); if ((v_at_a_zero > vMax && jMax > 0) || (v_at_a_zero < vMax && jMax < 0)) { - velocity_brake(v0, a0, vMax, vMin, aMax, jMax, t_brake, j_brake); + velocity_brake(v0, a0, vMax, vMin, aMax, aMin, jMax, t_brake, j_brake); } else if ((v_at_a_max < vMin && jMax > 0) || (v_at_a_max > vMin && jMax < 0)) { const double t_to_v_max = -(v_at_a_max + vMax)/aMax; @@ -35,9 +35,9 @@ void Brake::acceleration_brake(double v0, double a0, double vMax, double vMin, d } } -void Brake::velocity_brake(double v0, double a0, double vMax, double vMin, double aMax, double jMax, std::array& t_brake, std::array& j_brake) { +void Brake::velocity_brake(double v0, double a0, double vMax, double vMin, double aMax, double aMin, double jMax, std::array& t_brake, std::array& j_brake) { j_brake[0] = -jMax; - const double t_to_a_min = (a0 + aMax)/jMax; + const double t_to_a_min = (a0 - aMin)/jMax; const double t_to_v_max = a0/jMax + std::sqrt(a0*a0 + 2 * jMax * (v0 - vMax)) / std::abs(jMax); const double t_to_v_min = a0/jMax + std::sqrt(a0*a0/2 + jMax * (v0 - vMin)) / std::abs(jMax); const double t_min_to_v_max = std::min(t_to_v_max, t_to_v_min); @@ -55,23 +55,23 @@ void Brake::velocity_brake(double v0, double a0, double vMax, double vMin, doubl } } -void Brake::get_brake_trajectory(double v0, double a0, double vMax, double vMin, double aMax, double jMax, std::array& t_brake, std::array& j_brake) { +void Brake::get_brake_trajectory(double v0, double a0, double vMax, double vMin, double aMax, double aMin, double jMax, std::array& t_brake, std::array& j_brake) { t_brake[0] = 0.0; t_brake[1] = 0.0; j_brake[0] = 0.0; j_brake[1] = 0.0; if (a0 > aMax) { - acceleration_brake(v0, a0, vMax, vMin, aMax, jMax, t_brake, j_brake); + acceleration_brake(v0, a0, vMax, vMin, aMax, aMin, jMax, t_brake, j_brake); - } else if (a0 < -aMax) { - acceleration_brake(v0, a0, vMin, vMax, -aMax, -jMax, t_brake, j_brake); + } else if (a0 < aMin) { + acceleration_brake(v0, a0, vMin, vMax, aMin, aMax, -jMax, t_brake, j_brake); } else if ((v0 > vMax && v_at_a_zero(v0, a0, -jMax) > vMin) || (a0 > 0 && v_at_a_zero(v0, a0, jMax) > vMax)) { - velocity_brake(v0, a0, vMax, vMin, aMax, jMax, t_brake, j_brake); + velocity_brake(v0, a0, vMax, vMin, aMax, aMin, jMax, t_brake, j_brake); } else if ((v0 < vMin && v_at_a_zero(v0, a0, jMax) < vMax) || (a0 < 0 && v_at_a_zero(v0, a0, -jMax) < vMin)) { - velocity_brake(v0, a0, vMin, vMax, -aMax, -jMax, t_brake, j_brake); + velocity_brake(v0, a0, vMin, vMax, aMin, aMax, -jMax, t_brake, j_brake); } } diff --git a/src/step1.cpp b/src/step1.cpp index cd6ed753..fdb181a1 100644 --- a/src/step1.cpp +++ b/src/step1.cpp @@ -6,7 +6,7 @@ namespace ruckig { -Step1::Step1(double p0, double v0, double a0, double pf, double vf, double af, double vMax, double vMin, double aMax, double jMax): p0(p0), v0(v0), a0(a0), pf(pf), vf(vf), af(af), vMax(vMax), vMin(vMin), aMax(aMax), jMax(jMax) { +Step1::Step1(double p0, double v0, double a0, double pf, double vf, double af, double vMax, double vMin, double aMax, double aMin, double jMax): p0(p0), v0(v0), a0(a0), pf(pf), vf(vf), af(af), vMax(vMax), vMin(vMin), aMax(aMax), aMin(aMin), jMax(jMax) { pd = pf - p0; v0_v0 = v0 * v0; @@ -25,25 +25,24 @@ Step1::Step1(double p0, double v0, double a0, double pf, double vf, double af, d af_p6 = af_p4 * af_af; // max values needs to be invariant to plus minus sign change - aMax_aMax = aMax * aMax; jMax_jMax = jMax * jMax; } -void Step1::time_up_acc0_acc1_vel(Profile& profile, double vMax, double aMax, double jMax) { +void Step1::time_acc0_acc1_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax) { profile.t[0] = (-a0 + aMax)/jMax; - profile.t[1] = (a0_a0/2 - aMax_aMax - jMax*(v0 - vMax))/(aMax*jMax); + profile.t[1] = (a0_a0/2 - aMax*aMax - jMax*(v0 - vMax))/(aMax*jMax); profile.t[2] = aMax/jMax; - profile.t[3] = (3*(a0_p4 + af_p4) + 8*aMax*(af_p3 - a0_p3) + 24*aMax*jMax*(a0*v0 - af*vf) + 6*a0_a0*(aMax_aMax - 2*jMax*v0) + 6*af_af*(aMax_aMax - 2*jMax*vf) - 12*jMax*(aMax_aMax*(v0 + vf + 2*vMax) - jMax*(2*aMax*pd + v0_v0 + vf_vf - 2*vMax*vMax)))/(24*aMax*jMax_jMax*vMax); - profile.t[4] = profile.t[2]; - profile.t[5] = (af_af/2 - aMax_aMax - jMax*(vf - vMax))/(aMax*jMax); + profile.t[3] = (3*a0_p4*aMin - 3*af_p4*aMax + 8*(af_p3 - a0_p3)*aMax*aMin + 24*aMax*aMin*jMax*(a0*v0 - af*vf) + 6*a0_a0*aMin*(aMax*aMax - 2*jMax*v0) - 6*af_af*aMax*(aMin*aMin - 2*jMax*vf) - 12*jMax*(aMax*aMax*aMin*(v0 + vMax) + aMin*jMax*(vMax*vMax - v0_v0) - aMax*(2*aMin*jMax*pd + aMin*aMin*(vf + vMax) + jMax*(vMax*vMax - vf_vf))))/(24*aMax*aMin*jMax_jMax*vMax); + profile.t[4] = -aMin/jMax; + profile.t[5] = -(af_af/2 - aMin*aMin - jMax*(vf - vMax))/(aMin*jMax); profile.t[6] = profile.t[4] + af/jMax; - if (profile.check(pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(pf, vf, af, jMax, vMax, aMax, aMin)) { add_profile(profile, jMax); } } -void Step1::time_up_acc1_vel(Profile& profile, double vMax, double aMax, double jMax) { +void Step1::time_acc1_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax) { if ((jMax > 0 && has_up_vel) || (jMax < 0 && has_down_vel)) { return; } @@ -53,17 +52,17 @@ void Step1::time_up_acc1_vel(Profile& profile, double vMax, double aMax, double profile.t[0] = -a0/jMax + h1; profile.t[1] = 0; profile.t[2] = h1; - profile.t[3] = (3*af_p4 + 8*aMax*(af_p3 - a0_p3) + 24*aMax*jMax*(a0*v0 - af*vf) + 6*af_af*(aMax_aMax - 2*jMax*vf) - 12*jMax*(-2*aMax*jMax*pd + aMax_aMax*(vf + vMax) + jMax*(vMax*vMax - vf_vf) - aMax*h1*(a0_a0 - 2*jMax*(v0 + vMax))))/(24*aMax*jMax_jMax*vMax); - profile.t[4] = aMax/jMax; - profile.t[5] = (af_af/2 - aMax_aMax + jMax*(vMax - vf))/(aMax*jMax); + profile.t[3] = -(3*af_p4 - 8*aMin*(af_p3 - a0_p3) - 24*aMin*jMax*(a0*v0 - af*vf) + 6*af_af*(aMin*aMin - 2*jMax*vf) - 12*jMax*(2*aMin*jMax*pd + aMin*aMin*(vf + vMax) + jMax*(vMax*vMax - vf_vf) + aMin*h1*(a0_a0 - 2*jMax*(v0 + vMax))))/(24*aMin*jMax_jMax*vMax); + profile.t[4] = -aMin/jMax; + profile.t[5] = -(af_af/2 - aMin*aMin + jMax*(vMax - vf))/(aMin*jMax); profile.t[6] = profile.t[4] + af/jMax; - if (profile.check(pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(pf, vf, af, jMax, vMax, aMax, aMin)) { add_profile(profile, jMax); } } -void Step1::time_up_acc0_vel(Profile& profile, double vMax, double aMax, double jMax) { +void Step1::time_acc0_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax) { if ((jMax > 0 && has_up_vel) || (jMax < 0 && has_down_vel)) { return; } @@ -71,19 +70,19 @@ void Step1::time_up_acc0_vel(Profile& profile, double vMax, double aMax, double const double h1 = Sqrt(af_af/2 + jMax*(vMax - vf))/Abs(jMax); profile.t[0] = (-a0 + aMax)/jMax; - profile.t[1] = (a0_a0/2 - aMax_aMax - jMax*(v0 - vMax))/(aMax*jMax); + profile.t[1] = (a0_a0/2 - aMax*aMax - jMax*(v0 - vMax))/(aMax*jMax); profile.t[2] = aMax/jMax; - profile.t[3] = (3*a0_p4 + 8*(af_p3 - a0_p3)*aMax + 24*aMax*jMax*(a0*v0 - af*vf) + 6*a0_a0*(aMax_aMax - 2*jMax*v0) - 12*jMax*(-2*aMax*jMax*pd + aMax_aMax*(v0 + vMax) + jMax*(vMax*vMax - v0_v0) + (2*(vf + vMax)*jMax - af_af)*aMax*h1))/(24*aMax*jMax_jMax*vMax); + profile.t[3] = (3*a0_p4 + 8*(af_p3 - a0_p3)*aMax + 24*aMax*jMax*(a0*v0 - af*vf) + 6*a0_a0*(aMax*aMax - 2*jMax*v0) - 12*jMax*(-2*aMax*jMax*pd + aMax*aMax*(v0 + vMax) + jMax*(vMax*vMax - v0_v0) + (2*(vf + vMax)*jMax - af_af)*aMax*h1))/(24*aMax*jMax_jMax*vMax); profile.t[4] = h1; profile.t[5] = 0; profile.t[6] = profile.t[4] + af/jMax; - if (profile.check(pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(pf, vf, af, jMax, vMax, aMax, aMin)) { add_profile(profile, jMax); } } -void Step1::time_up_vel(Profile& profile, double vMax, double aMax, double jMax) { +void Step1::time_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax) { if ((jMax > 0 && has_up_vel) || (jMax < 0 && has_down_vel)) { return; } @@ -100,29 +99,26 @@ void Step1::time_up_vel(Profile& profile, double vMax, double aMax, double jMax) profile.t[5] = 0; profile.t[6] = profile.t[4] + af/jMax; - if (profile.check(pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(pf, vf, af, jMax, vMax, aMax, aMin)) { add_profile(profile, jMax); } } -void Step1::time_up_acc0_acc1(Profile& profile, double vMax, double aMax, double jMax) { - const double h1 = Sqrt((a0_p4 + af_p4)/2 + 4./3*(af_p3 - a0_p3)*aMax + 4*aMax*jMax*(a0*v0 - af*vf) + a0_a0*(aMax_aMax - 2*jMax*v0) + af_af*(aMax_aMax - 2*jMax*vf) + aMax_aMax*(aMax_aMax - 2*jMax*(v0 + vf)) + 2*jMax_jMax*(v0_v0 + vf_vf + 2*aMax*pd)); - - if (!std::isnan(h1)) { - const double h2 = a0_a0 - 3*aMax_aMax - 2*jMax*v0; - const double h3 = ((af_af - a0_a0)/2 - jMax*(vf - v0))/(aMax*jMax); +void Step1::time_acc0_acc1(Profile& profile, double vMax, double aMax, double aMin, double jMax) { + const double h1 = Sqrt(3)*Sqrt((aMax - aMin)*(3*Power(af,4)*aMax - 3*Power(a0,4)*aMin + 8*Power(a0,3)*aMax*aMin - 8*Power(af,3)*aMax*aMin - 24*a0*aMax*aMin*jMax*v0 - 6*Power(a0,2)*aMin*(Power(aMax,2) - 2*jMax*v0) + 24*af*aMax*aMin*jMax*vf + 6*Power(af,2)*aMax*(Power(aMin,2) - 2*jMax*vf) + 3*(Power(aMax,3)*Power(aMin,2) - 4*aMin*Power(jMax,2)*Power(v0,2) - Power(aMax,2)*(Power(aMin,3) - 4*aMin*jMax*v0) + 4*aMax*jMax*(2*aMin*jMax*(p0 - pf) - Power(aMin,2)*vf + jMax*Power(vf,2)))))*Abs(jMax)/(3*(aMax - aMin)*jMax); + if (!std::isnan(h1)) { // UDDU: Solution 2 { profile.t[0] = (-a0 + aMax)/jMax; - profile.t[1] = (h2 - h1)/(2*aMax*jMax); + profile.t[1] = (a0_a0 + aMax*aMin - 2*(aMax*aMax + jMax*v0) - h1)/(2*aMax*jMax); profile.t[2] = aMax/jMax; profile.t[3] = 0; - profile.t[4] = profile.t[2]; - profile.t[5] = profile.t[1] + h3; + profile.t[4] = -aMin/jMax; + profile.t[5] = -(af_af + aMax*aMin - 2*(aMin*aMin + jMax*vf) - h1)/(2*aMin*jMax); profile.t[6] = profile.t[4] + af/jMax; - if (profile.check(pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(pf, vf, af, jMax, vMax, aMax, aMin)) { add_profile(profile, jMax); } } @@ -130,27 +126,27 @@ void Step1::time_up_acc0_acc1(Profile& profile, double vMax, double aMax, double // UDDU: Solution 1 { profile.t[0] = (-a0 + aMax)/jMax; - profile.t[1] = (h2 + h1)/(2*aMax*jMax); + profile.t[1] = (a0_a0 + aMax*aMin - 2*(aMax*aMax + jMax*v0) + h1)/(2*aMax*jMax); profile.t[2] = aMax/jMax; profile.t[3] = 0; - profile.t[4] = profile.t[2]; - profile.t[5] = profile.t[1] + h3; + profile.t[4] = -aMin/jMax; + profile.t[5] = -(af_af + aMax*aMin - 2*(aMin*aMin + jMax*vf) + h1)/(2*aMin*jMax); profile.t[6] = profile.t[4] + af/jMax; - if (profile.check(pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(pf, vf, af, jMax, vMax, aMax, aMin)) { add_profile(profile, jMax); } } } } -void Step1::time_up_acc1(Profile& profile, double vMax, double aMax, double jMax) { +void Step1::time_acc1(Profile& profile, double vMax, double aMax, double aMin, double jMax) { std::array polynom; polynom[0] = 1.0; - polynom[1] = (2*(2*a0 + aMax))/jMax; - polynom[2] = (5*a0_a0 + 6*a0*aMax + aMax_aMax + 2*jMax*v0)/jMax_jMax; - polynom[3] = (2*(a0 + aMax)*(a0_a0 + a0*aMax + 2*jMax*v0))/(jMax_jMax*jMax); - polynom[4] = (3*(a0_p4 - af_p4) + 8*(a0_p3 - af_p3)*aMax + 24*aMax*jMax*(a0*v0 + af*vf) + 6*a0_a0*(aMax_aMax + 2*jMax*v0) - 6*af_af*(aMax_aMax - 2*jMax*vf) + 12*jMax*(-2*aMax*jMax*pd + aMax_aMax*(v0 + vf) + jMax*(v0_v0 - vf_vf)))/(12*jMax_jMax*jMax_jMax); + polynom[1] = (2*(2*a0 - aMin))/jMax; + polynom[2] = (5*a0_a0 - 6*a0*aMin + aMin*aMin + 2*jMax*v0)/jMax_jMax; + polynom[3] = (2*(a0 - aMin)*(a0_a0 - a0*aMin + 2*jMax*v0))/(jMax_jMax*jMax); + polynom[4] = (3*(a0_p4 - af_p4) - 8*(a0_p3 - af_p3)*aMin - 24*aMin*jMax*(a0*v0 + af*vf) + 6*a0_a0*(aMin*aMin + 2*jMax*v0) - 6*af_af*(aMin*aMin - 2*jMax*vf) + 12*jMax*(2*aMin*jMax*pd + aMin*aMin*(v0 + vf) + jMax*(v0_v0 - vf_vf)))/(12*jMax_jMax*jMax_jMax); auto roots = Roots::solveQuartMonic(polynom); for (double t: roots) { @@ -161,40 +157,40 @@ void Step1::time_up_acc1(Profile& profile, double vMax, double aMax, double jMax // Single Newton step (regarding pd) { double h1 = jMax*t*t + v0; - double orig = -pd + (3*(a0_p4 - af_p4) - 8*af_p3*aMax + 8*a0_p3*(aMax + 3*jMax*t) + 24*a0*jMax*(aMax + 2*jMax*t)*(aMax*t + h1) + 6*a0_a0*(aMax_aMax + 2*jMax*(4*aMax*t + 5*h1 - 4*v0)) + 24*af*aMax*jMax*vf - 6*af_af*(aMax_aMax - 2*jMax*vf) + 12*jMax*(2*aMax*jMax*t*(h1 + v0) + aMax_aMax*(h1 + vf) + jMax*(h1*h1 - vf_vf)))/(24*aMax*jMax_jMax); - double deriv = ((a0 + aMax + jMax*t)*(a0_a0 + aMax*jMax*t + a0*(aMax + 4*jMax*t) + 2*jMax*h1))/(aMax*jMax); + double orig = -pd + (3*(a0_p4 - af_p4) + 8*af_p3*aMin + 8*a0_p3*(-aMin + 3*jMax*t) + 24*a0*jMax*(-aMin + 2*jMax*t)*(-aMin*t + h1) + 6*a0_a0*(aMin*aMin + 2*jMax*(-4*aMin*t + 5*h1 - 4*v0)) - 24*af*aMin*jMax*vf - 6*af_af*(aMin*aMin - 2*jMax*vf) + 12*jMax*(-2*aMin*jMax*t*(h1 + v0) + aMin*aMin*(h1 + vf) + jMax*(h1*h1 - vf_vf)))/(-24*aMin*jMax_jMax); + double deriv = -((a0 - aMin + jMax*t)*(a0_a0 - aMin*jMax*t + a0*(-aMin + 4*jMax*t) + 2*jMax*h1))/(aMin*jMax); t -= orig / deriv; } // Corresponds to inverse ACC0 - if (t < DBL_EPSILON && (-af_af + 2*jMax*(vf - v0) + aMax_aMax) < DBL_EPSILON) { - continue; - } + // if (t < DBL_EPSILON && (-af_af + 2*jMax*(vf - v0) + aMin*aMin) < DBL_EPSILON) { + // continue; + // } profile.t[0] = t; profile.t[1] = 0; - profile.t[2] = (a0 + aMax)/jMax + t; + profile.t[2] = (a0 - aMin)/jMax + t; profile.t[3] = 0; profile.t[4] = 0; - profile.t[5] = ((a0_a0 + af_af)/2 - aMax_aMax + jMax*t*(2*a0 + jMax*t) - jMax*(vf - v0))/(aMax*jMax); - profile.t[6] = (af + aMax)/jMax; + profile.t[5] = -((a0_a0 + af_af)/2 - aMin*aMin + jMax*t*(2*a0 + jMax*t) - jMax*(vf - v0))/(aMin*jMax); + profile.t[6] = (af - aMin)/jMax; - if (profile.check(pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(pf, vf, af, jMax, vMax, aMax, aMin)) { add_profile(profile, jMax); } } } -void Step1::time_up_acc0(Profile& profile, double vMax, double aMax, double jMax) { +void Step1::time_acc0(Profile& profile, double vMax, double aMax, double aMin, double jMax) { // Combined UDDU and UDUD // UDUD Strategy t7 == 0 => is equal to UDDU std::array polynom; polynom[0] = 1.0; polynom[1] = (-2*aMax)/jMax; - polynom[2] = (-af_af + aMax_aMax + 2*jMax*vf)/jMax_jMax; + polynom[2] = (-af_af + aMax*aMax + 2*jMax*vf)/jMax_jMax; polynom[3] = 0; - polynom[4] = (3*(af_p4 - a0_p4) + 8*(a0_p3 - af_p3)*aMax + 24*aMax*jMax*(af*vf - a0*v0) - 6*a0_a0*(aMax_aMax - 2*jMax*v0) + 6*af_af*(aMax_aMax - 2*jMax*vf) + 12*jMax*(jMax*(vf_vf - v0_v0 - 2*aMax*pd) - aMax_aMax*(vf - v0)))/(12*jMax_jMax*jMax_jMax); + polynom[4] = (3*(af_p4 - a0_p4) + 8*(a0_p3 - af_p3)*aMax + 24*aMax*jMax*(af*vf - a0*v0) - 6*a0_a0*(aMax*aMax - 2*jMax*v0) + 6*af_af*(aMax*aMax - 2*jMax*vf) + 12*jMax*(jMax*(vf_vf - v0_v0 - 2*aMax*pd) - aMax*aMax*(vf - v0)))/(12*jMax_jMax*jMax_jMax); auto roots = Roots::solveQuartMonic(polynom); for (double t: roots) { @@ -210,13 +206,13 @@ void Step1::time_up_acc0(Profile& profile, double vMax, double aMax, double jMax profile.t[5] = 0; profile.t[6] = (af - aMax)/jMax + t; - if (profile.check(pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(pf, vf, af, jMax, vMax, aMax, aMin)) { add_profile(profile, jMax); } } } -void Step1::time_up_none(Profile& profile, double vMax, double aMax, double jMax) { +void Step1::time_none(Profile& profile, double vMax, double aMax, double aMin, double jMax) { // UDDU { if (std::abs(v0) < DBL_EPSILON && std::abs(a0) < DBL_EPSILON && std::abs(vf) < DBL_EPSILON && std::abs(af) < DBL_EPSILON) { @@ -228,7 +224,7 @@ void Step1::time_up_none(Profile& profile, double vMax, double aMax, double jMax profile.t[5] = 0; profile.t[6] = profile.t[0]; - if (profile.check(pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(pf, vf, af, jMax, vMax, aMax, aMin)) { add_profile(profile, jMax); } return; @@ -247,7 +243,7 @@ void Step1::time_up_none(Profile& profile, double vMax, double aMax, double jMax profile.t[5] = 0; profile.t[6] = 0; - if (profile.check(pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(pf, vf, af, jMax, vMax, aMax, aMin)) { add_profile(profile, jMax); } } @@ -262,7 +258,7 @@ void Step1::time_up_none(Profile& profile, double vMax, double aMax, double jMax profile.t[5] = 0; profile.t[6] = 0; - if (profile.check(pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(pf, vf, af, jMax, vMax, aMax, aMin)) { add_profile(profile, jMax); } } @@ -295,45 +291,13 @@ void Step1::time_up_none(Profile& profile, double vMax, double aMax, double jMax profile.t[5] = 0; profile.t[6] = (af - a0)/jMax + t - profile.t[0]; - if (profile.check(pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(pf, vf, af, jMax, vMax, aMax, aMin)) { add_profile(profile, jMax); } } } } -void Step1::time_down_acc0_acc1_vel(Profile& profile, double vMin, double aMax, double jMax) { - return time_up_acc0_acc1_vel(profile, vMin, -aMax, -jMax); -} - -void Step1::time_down_acc1_vel(Profile& profile, double vMin, double aMax, double jMax) { - return time_up_acc1_vel(profile, vMin, -aMax, -jMax); -} - -void Step1::time_down_acc0_vel(Profile& profile, double vMin, double aMax, double jMax) { - return time_up_acc0_vel(profile, vMin, -aMax, -jMax); -} - -void Step1::time_down_vel(Profile& profile, double vMin, double aMax, double jMax) { - return time_up_vel(profile, vMin, -aMax, -jMax); -} - -void Step1::time_down_acc0_acc1(Profile& profile, double vMin, double aMax, double jMax) { - return time_up_acc0_acc1(profile, vMin, -aMax, -jMax); -} - -void Step1::time_down_acc1(Profile& profile, double vMin, double aMax, double jMax) { - return time_up_acc1(profile, vMin, -aMax, -jMax); -} - -void Step1::time_down_acc0(Profile& profile, double vMin, double aMax, double jMax) { - return time_up_acc0(profile, vMin, -aMax, -jMax); -} - -void Step1::time_down_none(Profile& profile, double vMin, double aMax, double jMax) { - return time_up_none(profile, vMin, -aMax, -jMax); -} - bool Step1::calculate_block(Block& block) const { // if (valid_profile_counter > 0) // { @@ -394,25 +358,25 @@ bool Step1::get_profile(const Profile& input, Block& block) { valid_profile_counter = 0; if (std::abs(pf - p0) < DBL_EPSILON && std::abs(v0) < DBL_EPSILON && std::abs(vf) < DBL_EPSILON && std::abs(a0) < DBL_EPSILON && std::abs(af) < DBL_EPSILON) { - time_up_none(profile, vMax, aMax, jMax); + time_none(profile, vMax, aMax, aMin, jMax); } else { - time_up_acc0_acc1_vel(profile, vMax, aMax, jMax); - time_down_acc0_acc1_vel(profile, vMin, aMax, jMax); - time_up_acc1_vel(profile, vMax, aMax, jMax); - time_down_acc1_vel(profile, vMin, aMax, jMax); - time_up_acc0_vel(profile, vMax, aMax, jMax); - time_down_acc0_vel(profile, vMin, aMax, jMax); - time_up_vel(profile, vMax, aMax, jMax); - time_down_vel(profile, vMin, aMax, jMax); - time_up_none(profile, vMax, aMax, jMax); - time_up_acc0(profile, vMax, aMax, jMax); - time_up_acc1(profile, vMax, aMax, jMax); - time_up_acc0_acc1(profile, vMax, aMax, jMax); - time_down_none(profile, vMin, aMax, jMax); - time_down_acc0(profile, vMin, aMax, jMax); - time_down_acc1(profile, vMin, aMax, jMax); - time_down_acc0_acc1(profile, vMin, aMax, jMax); + time_acc0_acc1_vel(profile, vMax, aMax, aMin, jMax); + time_acc0_acc1_vel(profile, vMin, aMin, aMax, -jMax); + time_acc1_vel(profile, vMax, aMax, aMin, jMax); + time_acc1_vel(profile, vMin, aMin, aMax, -jMax); + time_acc0_vel(profile, vMax, aMax, aMin, jMax); + time_acc0_vel(profile, vMin, aMin, aMax, -jMax); + time_vel(profile, vMax, aMax, aMin, jMax); + time_vel(profile, vMin, aMin, aMax, -jMax); + time_none(profile, vMax, aMax, aMin, jMax); + time_acc0(profile, vMax, aMax, aMin, jMax); + time_acc1(profile, vMax, aMax, aMin, jMax); + time_acc0_acc1(profile, vMax, aMax, aMin, jMax); + time_none(profile, vMin, aMin, aMax, -jMax); + time_acc0(profile, vMin, aMin, aMax, -jMax); + time_acc1(profile, vMin, aMin, aMax, -jMax); + time_acc0_acc1(profile, vMin, aMin, aMax, -jMax); } return calculate_block(block); diff --git a/src/step2.cpp b/src/step2.cpp index e4c745e8..b18a70c0 100644 --- a/src/step2.cpp +++ b/src/step2.cpp @@ -6,7 +6,7 @@ namespace ruckig { -Step2::Step2(double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double vMin, double aMax, double jMax): tf(tf), p0(p0), v0(v0), a0(a0), pf(pf), vf(vf), af(af), vMax(vMax), vMin(vMin), aMax(aMax), jMax(jMax) { +Step2::Step2(double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double vMin, double aMax, double aMin, double jMax): tf(tf), p0(p0), v0(v0), a0(a0), pf(pf), vf(vf), af(af), vMax(vMax), vMin(vMin), aMax(aMax), aMin(aMin), jMax(jMax) { pd = pf - p0; tf_tf = tf * tf; tf_p3 = tf_tf * tf; @@ -32,55 +32,50 @@ Step2::Step2(double tf, double p0, double v0, double a0, double pf, double vf, d af_p6 = af_p4 * af_af; // max values needs to be invariant to plus minus sign change - aMax_aMax = aMax * aMax; jMax_jMax = jMax * jMax; g1 = -pd + tf*v0; g2 = -2*pd + tf*(v0 + vf); } -bool Step2::time_up_acc0_acc1_vel(Profile& profile, double vMax, double aMax, double jMax) { - if (tf < std::max((-a0 + aMax)/jMax, 0.0) + 2*std::max(aMax/jMax, 0.0)) { - return false; - } +bool Step2::time_acc0_acc1_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax) { + // if (tf < std::max((-a0 + aMax)/jMax, 0.0) + 2*std::max(aMax/jMax, 0.0)) { + // return false; + // } - // Profile UDDU + // Profile UDDU, Solution 1 { - const double h0b = aMax_aMax + jMax*(vd - aMax*tf); - const double h0a = af_af + 2*(af*aMax + aMax_aMax - aMax*jMax*tf - jMax*vd); - const double h1 = Sqrt(-a0_p4 - af_p4 + 4./3*aMax*(a0_p3 - af_p3) + 2*a0*h0a*(a0 - 2*aMax) + 4*af*h0b*(af + 2*aMax) + 4*(aMax_aMax*aMax_aMax - 2*aMax_aMax*aMax*jMax*tf + aMax_aMax*jMax_jMax*tf_tf - jMax_jMax*vd_vd + 2*aMax*jMax_jMax*g2)); - const double h2 = 2*aMax*(ad + 3*aMax - jMax*tf) + h1; - const double h3 = 4*aMax*jMax; - const double h4 = a0_a0 - af_af + 2*jMax*vd; + const double h0a = af_af - 2*af*aMin + (aMin - aMax)*aMin + 2*aMin*jMax*tf - 2*jMax*vd; + const double h1 = Sqrt(aMax*aMin*jMax_jMax*(3*(a0_p4 + af_p4) - 4*a0_p3*(2*aMax + aMin) - 4*af_p3*(aMax + 2*aMin) + 6*a0_a0*aMax*(aMin + aMax) + 6*a0*(2*aMax - a0)*h0a - 6*af_af*(aMax*aMax - aMin*aMin - 2*aMax*(aMin + jMax*tf) + 2*jMax*vd) + 12*af*aMin*(aMax*aMax - aMax*(aMin + 2*jMax*tf) + 2*jMax*vd) + 3*(aMax*aMax*aMax*aMin - 2*aMax*aMax*(aMin*aMin + 2*aMin*jMax*tf - 2*jMax*vd) + 4*jMax*(2*aMin*jMax*(-pd + tf*v0) - aMin*aMin*vd + jMax*vd*vd) + aMax*(aMin*aMin*aMin + 4*aMin*aMin*jMax*tf + 4*aMin*jMax_jMax*tf_tf - 8*jMax_jMax*(-pd + tf*vf)))))/(Sqrt(3)*jMax); profile.t[0] = (-a0 + aMax)/jMax; - profile.t[1] = -(h2 - h4)/h3; - profile.t[2] = profile.t[0] + a0/jMax; - profile.t[3] = tf - (ad + 4*aMax)/jMax + 2*h2/h3; - profile.t[4] = profile.t[2]; - profile.t[5] = -(h2 + h4)/h3; + profile.t[1] = -((af_af - a0_a0)*aMax + 2*Power(aMax,3) + 2*(a0 - af)*aMax*aMin - 3*Power(aMax,2)*aMin + aMax*Power(aMin,2) + 2*aMax*aMin*jMax*tf - 2*aMax*jMax*vd + h1)/(2*aMax*(aMax - aMin)*jMax); + profile.t[2] = aMax/jMax; + profile.t[3] = -(aMax*aMin*(aMax - aMin) + h1)/(2*aMax*aMin*jMax); + profile.t[4] = -aMin/jMax; + profile.t[5] = tf - (profile.t[0] + profile.t[1] + profile.t[2] + profile.t[3] + 2*profile.t[4] + af/jMax); profile.t[6] = profile.t[4] + af/jMax; - if (profile.check(tf, pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, jMax, vMax, aMax, aMin)) { return true; } } // Profile UDUD { - const double h1 = 12*aMax*jMax*(a0_a0 + af_af - 2*(a0 + af)*aMax + 2*(aMax_aMax - aMax*jMax*tf + jMax*vd)); + const double h1 = 12*aMax*jMax*(a0_a0 + af_af - 2*(a0 + af)*aMax + 2*(aMax*aMax - aMax*jMax*tf + jMax*vd)); const double h2 = 3*(a0_p4 + af_p4) - 4*(a0_p3 + af_p3)*aMax; - const double h3 = -4*af_p3*aMax + 24*(a0 + af)*aMax_aMax*aMax - 6*(af_af + a0_a0)*(aMax_aMax - 2*jMax*vd) + 6*a0_a0*(af_af - 2*af*aMax - 2*aMax*jMax*tf) - 12*aMax_aMax*(2*aMax_aMax - 2*aMax*jMax*tf + jMax*vd) - 24*af*aMax*jMax*vd + 12*jMax_jMax*(2*aMax*g1 + vd_vd); + const double h3 = -4*af_p3*aMax + 24*(a0 + af)*aMax*aMax*aMax - 6*(af_af + a0_a0)*(aMax*aMax - 2*jMax*vd) + 6*a0_a0*(af_af - 2*af*aMax - 2*aMax*jMax*tf) - 12*aMax*aMax*(2*aMax*aMax - 2*aMax*jMax*tf + jMax*vd) - 24*af*aMax*jMax*vd + 12*jMax_jMax*(2*aMax*g1 + vd_vd); profile.t[0] = (-a0 + aMax)/jMax; profile.t[1] = (h2 + h3)/h1; profile.t[2] = profile.t[0] + a0/jMax; - profile.t[3] = -(a0_a0 + af_af - 2*aMax*(a0 + af + jMax*tf) + 4*aMax_aMax + 2*jMax*vd)/(2*aMax*jMax); + profile.t[3] = -(a0_a0 + af_af - 2*aMax*(a0 + af + jMax*tf) + 4*aMax*aMax + 2*jMax*vd)/(2*aMax*jMax); profile.t[4] = profile.t[2]; profile.t[5] = tf - (profile.t[0] + profile.t[1] + profile.t[2] + profile.t[3] + 2*profile.t[4] - af/jMax); profile.t[6] = profile.t[4] - af/jMax; - if (profile.check(tf, pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, jMax, vMax, aMax, aMin)) { return true; } } @@ -88,41 +83,41 @@ bool Step2::time_up_acc0_acc1_vel(Profile& profile, double vMax, double aMax, do return false; } -bool Step2::time_up_acc1_vel(Profile& profile, double vMax, double aMax, double jMax) { - if (tf < std::max(a0/jMax, 0.0) + std::max(aMax/jMax, 0.0)) { - return false; - } +bool Step2::time_acc1_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax) { + // if (tf < std::max(a0/jMax, 0.0) + std::max(aMax/jMax, 0.0)) { + // return false; + // } // Profile UDDU { - const double ph1 = a0_a0 + af_af + aMax*(a0 + 2*af) + aMax_aMax - 2*jMax*(vd + aMax*tf); - const double ph2 = -2*aMax*(jMax*g1 + af*vd) - aMax_aMax*vd + jMax*vd_vd; - const double ph3 = af_af + 2*af*aMax + aMax_aMax - 2*jMax*(vd + aMax*tf); + const double ph1 = a0_a0 + af_af - aMin*(a0 + 2*af) + aMin*aMin - 2*jMax*(vd - aMin*tf); + const double ph2 = 2*aMin*(jMax*g1 + af*vd) - aMin*aMin*vd + jMax*vd_vd; + const double ph3 = af_af - 2*af*aMin + aMin*aMin - 2*jMax*(vd - aMin*tf); std::array polynom; polynom[0] = 1.0; - polynom[1] = (2*(2*a0 + aMax))/jMax; - polynom[2] = (4*a0_a0 + ph1 + 3*a0*aMax)/jMax_jMax; + polynom[1] = (2*(2*a0 - aMin))/jMax; + polynom[2] = (4*a0_a0 + ph1 - 3*a0*aMin)/jMax_jMax; polynom[3] = (2*a0*ph1)/(jMax_jMax*jMax); - polynom[4] = (3*(a0_p4 + af_p4) + 4*(a0_p3 + 2*af_p3)*aMax + 6*af_af*(aMax_aMax - 2*jMax*vd) + 12*jMax*ph2 + 6*a0_a0*ph3)/(12*jMax_jMax*jMax_jMax); + polynom[4] = (3*(a0_p4 + af_p4) - 4*(a0_p3 + 2*af_p3)*aMin + 6*af_af*(aMin*aMin - 2*jMax*vd) + 12*jMax*ph2 + 6*a0_a0*ph3)/(12*jMax_jMax*jMax_jMax); auto roots = Roots::solveQuartMonic(polynom); for (double t: roots) { - if (t < 0.0 || t > tf - aMax/jMax) { + if (t < 0.0 || t > tf + aMin/jMax) { continue; } - const double h1 = ((a0_a0 + af_af)/2 + jMax*(-vd + 2*a0*t + jMax*t*t))/aMax; + const double h1 = -((a0_a0 + af_af)/2 + jMax*(-vd + 2*a0*t + jMax*t*t))/aMin; profile.t[0] = t; profile.t[1] = 0; profile.t[2] = profile.t[0] + a0/jMax; - profile.t[3] = tf - (h1 + aMax + a0 + af)/jMax - 2*t; - profile.t[4] = aMax/jMax; - profile.t[5] = (h1 - aMax)/jMax; + profile.t[3] = tf - (h1 - aMin + a0 + af)/jMax - 2*t; + profile.t[4] = -aMin/jMax; + profile.t[5] = (h1 + aMin)/jMax; profile.t[6] = profile.t[4] + af/jMax; - if (profile.check(tf, pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, jMax, vMax, aMax, aMin)) { return true; } } @@ -130,10 +125,10 @@ bool Step2::time_up_acc1_vel(Profile& profile, double vMax, double aMax, double // Profile UDUD { - const double ph1 = a0_a0 - af_af + (2*af - a0)*aMax - aMax_aMax - 2*jMax*(vd - aMax*tf); - const double ph2 = aMax_aMax + 2*jMax*vd; + const double ph1 = a0_a0 - af_af + (2*af - a0)*aMax - aMax*aMax - 2*jMax*(vd - aMax*tf); + const double ph2 = aMax*aMax + 2*jMax*vd; const double ph3 = af_af + ph2 - 2*aMax*(af + jMax*tf); - const double ph4 = 2*aMax*jMax*g1 + aMax_aMax*vd + jMax*vd_vd; + const double ph4 = 2*aMax*jMax*g1 + aMax*aMax*vd + jMax*vd_vd; std::array polynom; polynom[0] = 1.0; @@ -158,7 +153,7 @@ bool Step2::time_up_acc1_vel(Profile& profile, double vMax, double aMax, double profile.t[5] = -(h1 + aMax)/jMax; profile.t[6] = profile.t[4] - af/jMax; - if (profile.check(tf, pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, jMax, vMax, aMax, aMin)) { return true; } } @@ -167,21 +162,21 @@ bool Step2::time_up_acc1_vel(Profile& profile, double vMax, double aMax, double return false; } -bool Step2::time_up_acc0_vel(Profile& profile, double vMax, double aMax, double jMax) { +bool Step2::time_acc0_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax) { if (tf < std::max((-a0 + aMax)/jMax, 0.0) + std::max(aMax/jMax, 0.0)) { return false; } - const double ph1 = 12*jMax*(-aMax_aMax*vd - jMax*vd_vd + 2*aMax*jMax*(-pd + tf*vf)); + const double ph1 = 12*jMax*(-aMax*aMax*vd - jMax*vd_vd + 2*aMax*jMax*(-pd + tf*vf)); // Profile UDDU { std::array polynom; polynom[0] = 1.0; polynom[1] = (2*aMax)/jMax; - polynom[2] = (a0_a0 - af_af + 2*ad*aMax + aMax_aMax + 2*jMax*(vd - aMax*tf))/jMax_jMax; + polynom[2] = (a0_a0 - af_af + 2*ad*aMax + aMax*aMax + 2*jMax*(vd - aMax*tf))/jMax_jMax; polynom[3] = 0; - polynom[4] = -(-3*(a0_p4 + af_p4) + 4*(af_p3 + 2*a0_p3)*aMax - 12*a0*aMax*(af_af - 2*jMax*vd) + 6*a0_a0*(af_af - aMax_aMax - 2*jMax*vd) + 6*af_af*(aMax_aMax - 2*aMax*jMax*tf + 2*jMax*vd) + ph1)/(12*jMax_jMax*jMax_jMax); + polynom[4] = -(-3*(a0_p4 + af_p4) + 4*(af_p3 + 2*a0_p3)*aMax - 12*a0*aMax*(af_af - 2*jMax*vd) + 6*a0_a0*(af_af - aMax*aMax - 2*jMax*vd) + 6*af_af*(aMax*aMax - 2*aMax*jMax*tf + 2*jMax*vd) + ph1)/(12*jMax_jMax*jMax_jMax); auto roots = Roots::solveQuartMonic(polynom); for (double t: roots) { @@ -192,8 +187,8 @@ bool Step2::time_up_acc0_vel(Profile& profile, double vMax, double aMax, double // Single Newton step (regarding pd) { double h1 = jMax*t*t + vd; - double orig = (-3*(a0_p4 + af_p4) + 4*(af_p3 + 2*a0_p3)*aMax - 24*af*aMax*jMax_jMax*t*t - 12*a0*aMax*(af_af - 2*jMax*h1) + 6*a0_a0*(af_af - aMax_aMax - 2*jMax*h1) + 6*af_af*(aMax_aMax - 2*aMax*jMax*tf + 2*jMax*h1) - 12*jMax*(aMax_aMax*h1 + jMax*h1*h1 + 2*aMax*jMax*(pd + jMax*t*t*(t - tf) - tf*vf)))/(24*aMax*jMax_jMax); - double deriv = -t*(a0_a0 - af_af + 2*aMax*(ad - jMax*tf) + aMax_aMax + 3*aMax*jMax*t + 2*jMax*h1)/aMax; + double orig = (-3*(a0_p4 + af_p4) + 4*(af_p3 + 2*a0_p3)*aMax - 24*af*aMax*jMax_jMax*t*t - 12*a0*aMax*(af_af - 2*jMax*h1) + 6*a0_a0*(af_af - aMax*aMax - 2*jMax*h1) + 6*af_af*(aMax*aMax - 2*aMax*jMax*tf + 2*jMax*h1) - 12*jMax*(aMax*aMax*h1 + jMax*h1*h1 + 2*aMax*jMax*(pd + jMax*t*t*(t - tf) - tf*vf)))/(24*aMax*jMax_jMax); + double deriv = -t*(a0_a0 - af_af + 2*aMax*(ad - jMax*tf) + aMax*aMax + 3*aMax*jMax*t + 2*jMax*h1)/aMax; t -= orig / deriv; } @@ -208,7 +203,7 @@ bool Step2::time_up_acc0_vel(Profile& profile, double vMax, double aMax, double profile.t[5] = 0; profile.t[6] = af/jMax + t; - if (profile.check(tf, pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, jMax, vMax, aMax, aMin)) { return true; } } @@ -219,9 +214,9 @@ bool Step2::time_up_acc0_vel(Profile& profile, double vMax, double aMax, double std::array polynom; polynom[0] = 1.0; polynom[1] = (-2*aMax)/jMax; - polynom[2] = -(a0_a0 + af_af - 2*(a0 + af)*aMax + aMax_aMax + 2*jMax*(vd - aMax*tf))/jMax_jMax; + polynom[2] = -(a0_a0 + af_af - 2*(a0 + af)*aMax + aMax*aMax + 2*jMax*(vd - aMax*tf))/jMax_jMax; polynom[3] = 0; - polynom[4] = (3*(a0_p4 + af_p4) - 4*(af_p3 + 2*a0_p3)*aMax + 6*a0_a0*(af_af + aMax_aMax + 2*jMax*vd) - 12*a0*aMax*(af_af + 2*jMax*vd) + 6*af_af*(aMax_aMax - 2*aMax*jMax*tf + 2*jMax*vd) - ph1)/(12*jMax_jMax*jMax_jMax); + polynom[4] = (3*(a0_p4 + af_p4) - 4*(af_p3 + 2*a0_p3)*aMax + 6*a0_a0*(af_af + aMax*aMax + 2*jMax*vd) - 12*a0*aMax*(af_af + 2*jMax*vd) + 6*af_af*(aMax*aMax - 2*aMax*jMax*tf + 2*jMax*vd) - ph1)/(12*jMax_jMax*jMax_jMax); auto roots = Roots::solveQuartMonic(polynom); for (double t: roots) { @@ -232,8 +227,8 @@ bool Step2::time_up_acc0_vel(Profile& profile, double vMax, double aMax, double // Single Newton step (regarding pd) { double h1 = jMax*t*t - vd; - double orig = -(3*(a0_p4 + af_p4) - 4*(2*a0_p3 + af_p3)*aMax + 24*af*aMax*jMax_jMax*t*t - 12*a0*aMax*(af_af - 2*jMax*h1) + 6*a0_a0*(af_af + aMax_aMax - 2*jMax*h1) + 6*af_af*(aMax_aMax - 2*jMax*(tf*aMax + h1)) + 12*jMax*(-aMax_aMax*h1 + jMax*h1*h1 - 2*aMax*jMax*(-pd + jMax*t*t*(t - tf) + tf*vf)))/(24*aMax*jMax_jMax); - double deriv = t*(a0_a0 + af_af - 2*jMax*h1 - 2*(a0 + af + jMax*tf)*aMax + aMax_aMax + 3*aMax*jMax*t)/aMax; + double orig = -(3*(a0_p4 + af_p4) - 4*(2*a0_p3 + af_p3)*aMax + 24*af*aMax*jMax_jMax*t*t - 12*a0*aMax*(af_af - 2*jMax*h1) + 6*a0_a0*(af_af + aMax*aMax - 2*jMax*h1) + 6*af_af*(aMax*aMax - 2*jMax*(tf*aMax + h1)) + 12*jMax*(-aMax*aMax*h1 + jMax*h1*h1 - 2*aMax*jMax*(-pd + jMax*t*t*(t - tf) + tf*vf)))/(24*aMax*jMax_jMax); + double deriv = t*(a0_a0 + af_af - 2*jMax*h1 - 2*(a0 + af + jMax*tf)*aMax + aMax*aMax + 3*aMax*jMax*t)/aMax; t -= orig / deriv; } @@ -248,7 +243,7 @@ bool Step2::time_up_acc0_vel(Profile& profile, double vMax, double aMax, double profile.t[5] = 0; profile.t[6] = -(af/jMax) + t; - if (profile.check(tf, pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, jMax, vMax, aMax, aMin)) { return true; } } @@ -257,7 +252,7 @@ bool Step2::time_up_acc0_vel(Profile& profile, double vMax, double aMax, double return false; } -bool Step2::time_up_vel(Profile& profile, double vMax, double aMax, double jMax) { +bool Step2::time_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax) { // Profile UDDU { const double p1 = af_af - 2*jMax*(-2*af*tf + jMax*tf_tf + 3*vd); @@ -331,7 +326,7 @@ bool Step2::time_up_vel(Profile& profile, double vMax, double aMax, double jMax) profile.t[5] = 0; profile.t[6] = profile.t[4] + af/jMax; - if (profile.check(tf, pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, jMax, vMax, aMax, aMin)) { return true; } } @@ -426,7 +421,7 @@ bool Step2::time_up_vel(Profile& profile, double vMax, double aMax, double jMax) profile.t[5] = 0; profile.t[6] = profile.t[4] - af/jMax; - if (profile.check(tf, pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, jMax, vMax, aMax, aMin)) { return true; } } @@ -435,72 +430,70 @@ bool Step2::time_up_vel(Profile& profile, double vMax, double aMax, double jMax) return false; } -bool Step2::time_up_acc0_acc1(Profile& profile, double vMax, double aMax, double jMax) { +bool Step2::time_acc0_acc1(Profile& profile, double vMax, double aMax, double aMin, double jMax) { if (std::abs(a0) < DBL_EPSILON && std::abs(af) < DBL_EPSILON) { - const double h1 = (v0 + vf)/aMax - (vd_vd + 4*aMax*pd)/(2*aMax_aMax*tf); + const double h1 = 2*aMin*(-pd + tf*v0) + vd*vd + aMax*(2*pd + aMin*tf_tf - 2*tf*vf); + const double h2 = ((aMax - aMin)*(-aMin*vd + aMax*(aMin*tf - vd))); - profile.t[0] = tf/2 + h1; - profile.t[1] = -(tf + 4*h1 - vd/aMax)/2; + const double jf = h2/h1; + profile.t[0] = aMax/jf; + profile.t[1] = (-2*aMax*h1 + aMin*aMin*(-2*pd + tf*(v0 + vf)))/h2; profile.t[2] = profile.t[0]; profile.t[3] = 0; - profile.t[4] = profile.t[0]; - profile.t[5] = -(tf + 4*h1 + vd/aMax)/2; - profile.t[6] = profile.t[0]; - const double jf = aMax/profile.t[0]; + profile.t[4] = -aMin/jf; + profile.t[5] = tf - (2*profile.t[0] + profile.t[1] + 2*profile.t[4]); + profile.t[6] = profile.t[4]; - return profile.check(tf, pf, vf, af, jf, vMax, aMax, jMax); + return profile.check(tf, pf, vf, af, jf, vMax, aMax, aMin, jMax); } - const double h1 = -2*ad_ad*(3*(a0_p3 - af_p3) - 4*(a0_a0 + af_af)*aMax + 12*(ad + 2*aMax)*aMax_aMax + af*a0*(3*(a0 - af) - 16*aMax)); - const double h2 = aMax_aMax*tf_tf - vd_vd + 2*aMax*g2; - const double h3 = 2*aMax_aMax*aMax*tf + (af_af + 2*af*aMax)*(aMax*tf - vd) + (a0_a0 - 2*a0*aMax)*(aMax*tf + vd); - const double h4 = Sqrt(h3*h3 - h1*h2/3); - const double jf = (h3 + h4)/(2*h2); + const double h1 = Sqrt(144*Power((aMax - aMin)*(-aMin*vd + aMax*(aMin*tf - vd)) - af_af*(aMax*tf - vd) + 2*af*aMin*(aMax*tf - vd) + a0_a0*(aMin*tf + v0 - vf) - 2*a0*aMax*(aMin*tf - vd),2) + 48*ad*(3*Power(a0,3) - 3*Power(af,3) + 12*aMax*aMin*(-aMax + aMin) + 4*af_af*(aMax + 2*aMin) + a0*(-3*af_af - 8*af*aMax + 6*Power(aMax,2) + 8*af*aMin + 12*aMax*aMin - 6*Power(aMin,2)) + 6*af*(Power(aMax,2) - 2*aMax*aMin - Power(aMin,2)) + a0_a0*(3*af - 4*(2*aMax + aMin)))*(2*aMin*(-pd + tf*v0) + vd*vd + aMax*(2*pd + aMin*tf*tf - 2*tf*vf))); - profile.t[0] = (-a0 + aMax)/jf; - profile.t[1] = (a0_a0 - af_af - 2*ad*aMax - 8*aMax_aMax + 6*aMax*tf + 6*vd)/(12*aMax); - profile.t[2] = profile.t[0] + a0/jf; + const double jf = -(3*af_af*aMax*tf - 3*a0_a0*aMin*tf - 6*ad*aMax*aMin*tf + 3*aMax*aMin*(aMin - aMax)*tf + 3*(a0_a0 - af_af)*vd + 6*af*aMin*vd - 6*a0*aMax*vd + 3*(aMax*aMax - aMin*aMin)*vd + h1/4)/(6*(2*aMin*(p0 - pf + tf*v0) + vd*vd + aMax*(2*pd + aMin*tf_tf - 2*tf*vf))); + profile.t[0] = (aMax - a0)/jf; + profile.t[1] = (a0_a0 - af_af + 2*ad*aMin - 2*(aMax*aMax - 2*aMax*aMin + aMin*aMin + aMin*jf*tf - jf*vd))/(2*(aMax - aMin)*jf); + profile.t[2] = aMax/jf; profile.t[3] = 0; - profile.t[4] = profile.t[2]; - profile.t[5] = tf - (profile.t[0] + profile.t[1] + profile.t[3] + 3*profile.t[2] + af/jf); + profile.t[4] = -aMin/jf; + profile.t[5] = tf - (profile.t[0] + profile.t[1] + profile.t[2] + 2*profile.t[4] + af/jf); profile.t[6] = profile.t[4] + af/jf; - - if (profile.check(tf, pf, vf, af, jMax, vMax, aMax)) { + + if (profile.check(tf, pf, vf, af, jf, vMax, aMax, aMin, jMax)) { return true; } return false; } -bool Step2::time_up_acc1(Profile& profile, double vMax, double aMax, double jMax) { +bool Step2::time_acc1(Profile& profile, double vMax, double aMax, double aMin, double jMax) { // a3 != 0 // Case UDDU, Solution 2 { - const double h0a = a0_p3 - af_p3 + 3*a0_a0*aMax + 3*a0*aMax_aMax + 3*aMax_aMax*jMax*tf - 3*af*aMax*(aMax - 2*jMax*tf) - 3*af_af*(aMax - jMax*tf) - 3*jMax_jMax*(-2*pd + aMax*tf_tf + 2*tf*vf); - const double h0b = a0_a0 + af_af + 2*(a0 + af)*aMax + 2*(aMax_aMax - jMax*(aMax*tf + vd)); - const double h0c = a0_p4 + 3*af_p4 + 4*(a0_p3 + 2*af_p3)*aMax + 6*a0_a0*aMax_aMax + 6*af_af*(aMax_aMax - 2*jMax*vd) + 12*jMax*(-2*aMax*jMax*g1 - aMax_aMax*vd + jMax*vd_vd) - 24*af*aMax*jMax*vd - 4*a0*(af_p3 + 3*af*aMax*(aMax - 2*jMax*tf) + 3*af_af*(aMax - jMax*tf) + 3*jMax*(-(aMax_aMax*tf) + jMax*(-2*pd + aMax*tf_tf + 2*tf*vf))); + const double h0a = a0_p3 - af_p3 - 3*a0_a0*aMin + 3*a0*aMin*aMin + 3*aMin*aMin*jMax*tf + 3*af*aMin*(-aMin - 2*jMax*tf) - 3*af_af*(-aMin - jMax*tf) - 3*jMax_jMax*(-2*pd - aMin*tf_tf + 2*tf*vf); + const double h0b = a0_a0 + af_af - 2*(a0 + af)*aMin + 2*(aMin*aMin - jMax*(-aMin*tf + vd)); + const double h0c = a0_p4 + 3*af_p4 - 4*(a0_p3 + 2*af_p3)*aMin + 6*a0_a0*aMin*aMin + 6*af_af*(aMin*aMin - 2*jMax*vd) + 12*jMax*(2*aMin*jMax*g1 - aMin*aMin*vd + jMax*vd_vd) + 24*af*aMin*jMax*vd - 4*a0*(af_p3 - 3*af*aMin*(-aMin - 2*jMax*tf) + 3*af_af*(-aMin - jMax*tf) + 3*jMax*(-aMin*aMin*tf + jMax*(-2*pd - aMin*tf_tf + 2*tf*vf))); const double h1 = Abs(jMax)/jMax*Sqrt(4*h0a*h0a - 6*h0b*h0c); const double h2 = 6*jMax*h0b; profile.t[0] = 0; profile.t[1] = 0; profile.t[2] = (2*h0a + h1)/h2; - profile.t[3] = -(a0_a0 + af_af + 2*(a0 + af)*aMax + 2*(aMax_aMax - aMax*jMax*tf - jMax*vd))/(2*jMax*(a0 + aMax - jMax*profile.t[2])); - profile.t[4] = (aMax + a0)/jMax - profile.t[2]; - profile.t[5] = tf - (profile.t[2] + profile.t[3] + profile.t[4] + (af + aMax)/jMax); - profile.t[6] = (af + aMax)/jMax; + profile.t[3] = -(a0_a0 + af_af - 2*(a0 + af)*aMin + 2*(aMin*aMin + aMin*jMax*tf - jMax*vd))/(2*jMax*(a0 - aMin - jMax*profile.t[2])); + profile.t[4] = (a0 - aMin)/jMax - profile.t[2]; + profile.t[5] = tf - (profile.t[2] + profile.t[3] + profile.t[4] + (af - aMin)/jMax); + profile.t[6] = (af - aMin)/jMax; - if (profile.check(tf, pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, jMax, vMax, aMax, aMin)) { return true; } } // Case UDUD, Solution 1 { - const double h0a = -a0_p3 + af_p3 + 3*a0_a0*aMax - 3*a0*aMax_aMax + 3*af*aMax*(aMax - 2*jMax*tf) - 3*af_af*(aMax - jMax*tf) + 3*jMax*(aMax_aMax*tf + jMax*(-2*pd - aMax*tf_tf + 2*tf*vf)); + const double h0a = -a0_p3 + af_p3 + 3*a0_a0*aMax - 3*a0*aMax*aMax + 3*af*aMax*(aMax - 2*jMax*tf) - 3*af_af*(aMax - jMax*tf) + 3*jMax*(aMax*aMax*tf + jMax*(-2*pd - aMax*tf_tf + 2*tf*vf)); const double h0b = a0_a0 - af_af + 2*ad*aMax + 2*jMax*(aMax*tf - vd); - const double h0c = a0_p4 + 3*af_p4 - 4*(a0_p3 + 2*af_p3)*aMax + 6*a0_a0*aMax_aMax - 24*af*aMax*jMax*vd + 12*jMax*(2*aMax*jMax*g1 + jMax*vd_vd + aMax_aMax*vd) + 6*af_af*(aMax_aMax + 2*jMax*vd) - 4*a0*(af_p3 + 3*af*aMax*(aMax - 2*jMax*tf) - 3*af_af*(aMax - jMax*tf) + 3*jMax*(aMax_aMax*tf + jMax*(-2*pd - aMax*tf_tf + 2*tf*vf))); + const double h0c = a0_p4 + 3*af_p4 - 4*(a0_p3 + 2*af_p3)*aMax + 6*a0_a0*aMax*aMax - 24*af*aMax*jMax*vd + 12*jMax*(2*aMax*jMax*g1 + jMax*vd_vd + aMax*aMax*vd) + 6*af_af*(aMax*aMax + 2*jMax*vd) - 4*a0*(af_p3 + 3*af*aMax*(aMax - 2*jMax*tf) - 3*af_af*(aMax - jMax*tf) + 3*jMax*(aMax*aMax*tf + jMax*(-2*pd - aMax*tf_tf + 2*tf*vf))); const double h1 = Abs(jMax)/jMax*Sqrt(4*h0a*h0a - 6*h0b*h0c); const double h2 = 6*jMax*h0b; @@ -512,20 +505,20 @@ bool Step2::time_up_acc1(Profile& profile, double vMax, double aMax, double jMax profile.t[5] = tf - (profile.t[2] + profile.t[3] + profile.t[4] + (-af + aMax)/jMax); profile.t[6] = (-af + aMax)/jMax; - if (profile.check(tf, pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, jMax, vMax, aMax, aMin)) { return true; } } return false; } -bool Step2::time_up_acc0(Profile& profile, double vMax, double aMax, double jMax) { +bool Step2::time_acc0(Profile& profile, double vMax, double aMax, double aMin, double jMax) { // a3 != 0 // UDDU Solution 1 { - const double h0a = a0_p3 + 2*af_p3 - 6*(af_af + aMax_aMax)*aMax - 6*(a0 + af)*aMax*jMax*tf + 9*aMax_aMax*(af + jMax*tf) + 3*a0*aMax*(-2*af + 3*aMax) + 3*a0_a0*(af - 2*aMax + jMax*tf) - 6*jMax_jMax*g1 + 6*(af - aMax)*jMax*vd - 3*aMax*jMax_jMax*tf_tf; - const double h0b = a0_a0 + af_af + 2*(aMax_aMax - (a0 + af)*aMax + jMax*(vd - aMax*tf)); + const double h0a = a0_p3 + 2*af_p3 - 6*(af_af + aMax*aMax)*aMax - 6*(a0 + af)*aMax*jMax*tf + 9*aMax*aMax*(af + jMax*tf) + 3*a0*aMax*(-2*af + 3*aMax) + 3*a0_a0*(af - 2*aMax + jMax*tf) - 6*jMax_jMax*g1 + 6*(af - aMax)*jMax*vd - 3*aMax*jMax_jMax*tf_tf; + const double h0b = a0_a0 + af_af + 2*(aMax*aMax - (a0 + af)*aMax + jMax*(vd - aMax*tf)); const double h1 = Abs(jMax)/jMax*Sqrt(4*h0a*h0a - 18*h0b*h0b*h0b); const double h2 = 6*jMax*h0b; @@ -537,7 +530,7 @@ bool Step2::time_up_acc0(Profile& profile, double vMax, double aMax, double jMax profile.t[5] = 0; profile.t[6] = 0; - if (profile.check(tf, pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, jMax, vMax, aMax, aMin)) { return true; } } @@ -545,7 +538,7 @@ bool Step2::time_up_acc0(Profile& profile, double vMax, double aMax, double jMax return false; } -bool Step2::time_up_none(Profile& profile, double vMax, double aMax, double jMax) { +bool Step2::time_none(Profile& profile, double vMax, double aMax, double aMin, double jMax) { if (std::abs(v0) < DBL_EPSILON && std::abs(a0) < DBL_EPSILON && std::abs(af) < DBL_EPSILON) { const double h1 = Sqrt(tf_tf*vf_vf + Power(4*pd - tf*vf,2)); const double jf = 4*(4*pd - 2*tf*vf + h1)/tf_p3; @@ -558,7 +551,7 @@ bool Step2::time_up_none(Profile& profile, double vMax, double aMax, double jMax profile.t[5] = 0; profile.t[6] = profile.t[0]; - if (profile.check(tf, pf, vf, af, jf, vMax, aMax, jMax)) { + if (profile.check(tf, pf, vf, af, jf, vMax, aMax, aMin, jMax)) { return true; } } @@ -577,7 +570,7 @@ bool Step2::time_up_none(Profile& profile, double vMax, double aMax, double jMax profile.t[5] = 0; profile.t[6] = profile.t[4]; - if (profile.check(tf, pf, vf, af, jf, vMax, aMax, jMax)) { + if (profile.check(tf, pf, vf, af, jf, vMax, aMax, aMin, jMax)) { return true; } } @@ -622,7 +615,7 @@ bool Step2::time_up_none(Profile& profile, double vMax, double aMax, double jMax profile.t[5] = 0; profile.t[6] = tf - (t + profile.t[3] + profile.t[4]); - if (profile.check(tf, pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, jMax, vMax, aMax, aMin)) { return true; } } @@ -667,7 +660,7 @@ bool Step2::time_up_none(Profile& profile, double vMax, double aMax, double jMax profile.t[5] = 0; profile.t[6] = 0; - if (profile.check(tf, pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, jMax, vMax, aMax, aMin)) { return true; } } @@ -716,7 +709,7 @@ bool Step2::time_up_none(Profile& profile, double vMax, double aMax, double jMax profile.t[5] = 0; profile.t[6] = tf - (t + profile.t[3] + profile.t[4]); - if (profile.check(tf, pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, jMax, vMax, aMax, aMin)) { return true; } } @@ -759,7 +752,7 @@ bool Step2::time_up_none(Profile& profile, double vMax, double aMax, double jMax profile.t[5] = 0; profile.t[6] = 0; - if (profile.check(tf, pf, vf, af, jMax, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, jMax, vMax, aMax, aMin)) { return true; } } @@ -781,7 +774,7 @@ bool Step2::time_up_none(Profile& profile, double vMax, double aMax, double jMax profile.t[5] = 0; profile.t[6] = 0; - if (profile.check(tf, pf, vf, af, jf, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, jf, vMax, aMax, aMin)) { return true; } } @@ -811,7 +804,7 @@ bool Step2::time_up_none(Profile& profile, double vMax, double aMax, double jMax profile.t[5] = 0; profile.t[6] = tf - (profile.t[0] + profile.t[1]); - if (profile.check(tf, pf, vf, af, jf, vMax, aMax)) { + if (profile.check(tf, pf, vf, af, jf, vMax, aMax, aMin)) { return true; } } @@ -820,38 +813,6 @@ bool Step2::time_up_none(Profile& profile, double vMax, double aMax, double jMax return false; } -bool Step2::time_down_acc0_acc1_vel(Profile& profile, double vMin, double aMax, double jMax) { - return time_up_acc0_acc1_vel(profile, vMin, -aMax, -jMax); -} - -bool Step2::time_down_acc1_vel(Profile& profile, double vMin, double aMax, double jMax) { - return time_up_acc1_vel(profile, vMin, -aMax, -jMax); -} - -bool Step2::time_down_acc0_vel(Profile& profile, double vMin, double aMax, double jMax) { - return time_up_acc0_vel(profile, vMin, -aMax, -jMax); -} - -bool Step2::time_down_vel(Profile& profile, double vMin, double aMax, double jMax) { - return time_up_vel(profile, vMin, -aMax, -jMax); -} - -bool Step2::time_down_acc0_acc1(Profile& profile, double vMin, double aMax, double jMax) { - return time_up_acc0_acc1(profile, vMin, -aMax, -jMax); -} - -bool Step2::time_down_acc1(Profile& profile, double vMin, double aMax, double jMax) { - return time_up_acc1(profile, vMin, -aMax, -jMax); -} - -bool Step2::time_down_acc0(Profile& profile, double vMin, double aMax, double jMax) { - return time_up_acc0(profile, vMin, -aMax, -jMax); -} - -bool Step2::time_down_none(Profile& profile, double vMin, double aMax, double jMax) { - return time_up_none(profile, vMin, -aMax, -jMax); -} - bool Step2::get_profile(Profile& profile) { profile.a[0] = a0; profile.v[0] = v0; @@ -860,40 +821,10 @@ bool Step2::get_profile(Profile& profile) { // Test all cases to get ones that match // However we should guess which one is correct and try them first... if (pf > p0 + v0 * tf) { - return time_up_acc0_acc1_vel(profile, vMax, aMax, jMax) - || time_up_acc0_vel(profile, vMax, aMax, jMax) - || time_up_acc1_vel(profile, vMax, aMax, jMax) - || time_up_vel(profile, vMax, aMax, jMax) - || time_up_acc0(profile, vMax, aMax, jMax) - || time_up_acc1(profile, vMax, aMax, jMax) - || time_up_acc0_acc1(profile, vMax, aMax, jMax) - || time_up_none(profile, vMax, aMax, jMax) - || time_down_acc0_acc1_vel(profile, vMin, aMax, jMax) - || time_down_acc0_vel(profile, vMin, aMax, jMax) - || time_down_acc1_vel(profile, vMin, aMax, jMax) - || time_down_vel(profile, vMin, aMax, jMax) - || time_down_acc0(profile, vMin, aMax, jMax) - || time_down_acc1(profile, vMin, aMax, jMax) - || time_down_acc0_acc1(profile, vMin, aMax, jMax) - || time_down_none(profile, vMin, aMax, jMax); + return check_all(profile, vMax, aMax, aMin, jMax) || check_all(profile, vMin, aMin, aMax, -jMax); } else { - return time_down_acc0_acc1_vel(profile, vMin, aMax, jMax) - || time_down_acc0_vel(profile, vMin, aMax, jMax) - || time_down_acc1_vel(profile, vMin, aMax, jMax) - || time_down_vel(profile, vMin, aMax, jMax) - || time_down_acc0(profile, vMin, aMax, jMax) - || time_down_acc1(profile, vMin, aMax, jMax) - || time_down_acc0_acc1(profile, vMin, aMax, jMax) - || time_down_none(profile, vMin, aMax, jMax) - || time_up_acc0_acc1_vel(profile, vMax, aMax, jMax) - || time_up_acc0_vel(profile, vMax, aMax, jMax) - || time_up_acc1_vel(profile, vMax, aMax, jMax) - || time_up_vel(profile, vMax, aMax, jMax) - || time_up_acc0(profile, vMax, aMax, jMax) - || time_up_acc1(profile, vMax, aMax, jMax) - || time_up_acc0_acc1(profile, vMax, aMax, jMax) - || time_up_none(profile, vMax, aMax, jMax); + return check_all(profile, vMin, aMin, aMax, -jMax) || check_all(profile, vMax, aMax, aMin, jMax); } } diff --git a/test/otg_plot.py b/test/otg_plot.py index 1a9927f8..175deef1 100644 --- a/test/otg_plot.py +++ b/test/otg_plot.py @@ -2,8 +2,8 @@ from pathlib import Path import sys -import matplotlib.pyplot as plt -import numpy as np +# import matplotlib.pyplot as plt +# import numpy as np sys.path.insert(0, str(Path(__file__).parent.parent / 'build')) @@ -106,15 +106,15 @@ def print_input_for_mathematica(inp, dof, tf=None): if __name__ == '__main__': inp = InputParameter() - inp.current_position = [0, 1, 1] - inp.current_velocity = [1, -1, -1] - inp.current_acceleration = [0, -1, 0] - inp.target_position = [1, -1, 0] - inp.target_velocity = [-1, 0, 0] - inp.target_acceleration = [-1, -1, 0] - inp.max_velocity = [3, 1, 1] - inp.max_acceleration = [2, 3, 1] - inp.max_jerk = [1, 1, 1] + inp.current_position = [-3.4892945933599, -4.69945035148771, 2.49204848752554] + inp.current_velocity = [0.65578126763966, 1.0884869483274, -0.0630881069389757] + inp.current_acceleration = [1.6380031144704, 0.77434883887819, -0.190312507264066] + inp.target_position = [0.232257762854349, -3.77571462366535, 0.786398044377225] + inp.target_velocity = [-0.430724968242918, 0, -0.935679763040163] + inp.target_acceleration = [0, 1.56136446402276, 0] + inp.max_velocity = [8.8151548981258, 10.4850574373953, 9.01164458430031] + inp.max_acceleration = [1.9162661732782, 2.51797431801212, 1.72576510848757] + inp.max_jerk = [3.05203308956307, 2.36750898434981, 1.65442703447078] print_input_for_mathematica(inp, 1) @@ -130,4 +130,4 @@ def print_input_for_mathematica(inp, dof, tf=None): print(f'Calculation duration: {out_list[0].calculation_duration:0.1f} [µs]') print(f'Trajectory duration: {out_list[0].trajectory.duration:0.4f} [s]') - plot_trajectory(t_list, out_list) + # plot_trajectory(t_list, out_list)