Skip to content

Commit

Permalink
improve acceleration target, readme
Browse files Browse the repository at this point in the history
  • Loading branch information
@pantor
pantor committed Dec 26, 2020
1 parent 4d0e94e commit 5533d34
Show file tree
Hide file tree
Showing 8 changed files with 225 additions and 113 deletions.
44 changes: 40 additions & 4 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,13 +40,16 @@ A python module can be built using the `BUILD_PYTHON_MODULE` CMake flag.

## Tutorial

Figure.
Figure. Currently only the (more-complex) *position* interface is implemented.

<div align="center">
<img width="500" src="https://raw.githubusercontent.com/pantor/ruckig/master/doc/example_profile.png?sanitize=true">
</div>

### Real-time trajectory generation

```c++
Ruckig<6> ruckig {0.001}; // DoF, control cycle in [s]
Ruckig<6> ruckig {0.001}; // Number DoFs; control cycle in [s]

InputParameter<6> input;
input.current_position = {};
Expand All @@ -73,16 +76,51 @@ while (otg.update(input, output) == Result::Working) {

### Input Type

```c++
std::array<double, DOFs> current_position;
std::array<double, DOFs> current_velocity {Vector::Zero()};
std::array<double, DOFs> current_acceleration {Vector::Zero()};

std::array<double, DOFs> target_position;
std::array<double, DOFs> target_velocity {Vector::Zero()};
std::array<double, DOFs> target_acceleration {Vector::Zero()};

std::array<double, DOFs> max_velocity;
std::array<double, DOFs> max_acceleration;
std::array<double, DOFs> max_jerk;

std::array<bool, DOFs> enabled;
std::optional<double> minimum_duration;
```
### Result Type
The `update` function of the Ruckig class returns a Result type that indicates the current state of the algorithm. Currently, this can either be **working**, **finished** if the trajectory has finished, or **error** if something went wrong during calculation. In this case, an exception (see below for more details) is thrown.
### Output Type
```c++
std::array<double, DOFs> new_position;
std::array<double, DOFs> new_velocity;
std::array<double, DOFs> new_acceleration;
double duration; // Duration of the trajectory [s]
bool new_calculation; // Whether a new calactuion was performed
double calculation_duration; // Duration of the calculation [µs]
```


### Exceptions


## Tests

The current test suite validates over 190.000 trajectories based on random inputs. Numeric stability is an issue.
Position, Velocity, and Acceleration target to `1e-8`, Velocity, Acceleration, and Jerk limits to `1e-9`.


## Development

Ruckig is written in C++17. It is currently tested against following versions
Expand All @@ -91,5 +129,3 @@ Ruckig is written in C++17. It is currently tested against following versions
- Catch2 v2.13.3 (only for testing)
- Reflexxes v1.2.7 (only for testing)
- Pybind11 v2.6.0 (only for prototyping)

The current test suite validates over 170.000 trajectories based on random inputs.
Binary file added doc/example_profile.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
2 changes: 0 additions & 2 deletions include/ruckig/ruckig.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -259,8 +259,6 @@ class Ruckig {

double t_profile = tf - profiles[dof].t_brake.value_or(0.0);

// std::cout << "step 2 for dof: " << dof << std::endl;

Step2 step2 {t_profile, p0s[dof], v0s[dof], a0s[dof], input.target_position[dof], input.target_velocity[dof], input.target_acceleration[dof], input.max_velocity[dof], input.max_acceleration[dof], input.max_jerk[dof]};
bool found_time_synchronization = step2.get_profile(profiles[dof], input.max_velocity[dof], input.max_acceleration[dof], input.max_jerk[dof]);
if (!found_time_synchronization) {
Expand Down
186 changes: 103 additions & 83 deletions notebooks/ruckig-step1.nb
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,10 +10,10 @@
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[ 158, 7]
NotebookDataLength[ 32404, 817]
NotebookOptionsPosition[ 30396, 779]
NotebookOutlinePosition[ 30790, 795]
CellTagsIndexPosition[ 30747, 792]
NotebookDataLength[ 33330, 837]
NotebookOptionsPosition[ 31321, 799]
NotebookOutlinePosition[ 31715, 815]
CellTagsIndexPosition[ 31672, 812]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Expand Down Expand Up @@ -45,7 +45,7 @@ Cell[BoxData[{
RowBox[{"-", "jMax"}]}], "}"}]}], ";"}],
"\[IndentingNewLine]"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"jerkProfile", "=", "jerkProfileUDDU"}],
RowBox[{"jerkProfile", "=", "jerkProfileUDUD"}],
";"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{
Expand Down Expand Up @@ -251,9 +251,8 @@ Cell[BoxData[{
3.8176136118348*^9, 3.8176136203885107`*^9}, 3.817613860612129*^9,
3.817615902502871*^9, 3.817628855244062*^9, 3.817628926525811*^9,
3.817630327779182*^9, 3.817630856882971*^9, 3.817631232893464*^9,
3.817631310598453*^9, 3.817640134759371*^9},
CellLabel->
"In[413]:=",ExpressionUUID->"d346b209-9dd3-47cf-8f1e-f04bc3bf980a"],
3.817631310598453*^9, 3.817640134759371*^9, 3.818009215253849*^9},
CellLabel->"In[1]:=",ExpressionUUID->"d346b209-9dd3-47cf-8f1e-f04bc3bf980a"],

Cell[BoxData[
RowBox[{
Expand Down Expand Up @@ -387,8 +386,7 @@ Cell[BoxData[
3.817629174090706*^9, {3.817629395753993*^9, 3.817629397914218*^9}, {
3.817629903651003*^9, 3.81762990789653*^9}, 3.817630008980739*^9, {
3.8176308336239557`*^9, 3.8176308355038147`*^9}, 3.8176412855214357`*^9},
CellLabel->
"In[438]:=",ExpressionUUID->"ccba75f7-892e-4119-93ec-41811b2e330e"],
CellLabel->"In[26]:=",ExpressionUUID->"ccba75f7-892e-4119-93ec-41811b2e330e"],

Cell["\<\
Information Roots are max. 4th Order
Expand Down Expand Up @@ -508,7 +506,7 @@ Cell[CellGroupData[{

Cell[BoxData[{
RowBox[{
RowBox[{"tRoot", "=", "5"}], ";"}], "\[IndentingNewLine]",
RowBox[{"tRoot", "=", "1"}], ";"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"tmpResult", "=",
RowBox[{"resultT", "[",
Expand Down Expand Up @@ -551,57 +549,60 @@ Cell[BoxData[{
3.817466215391354*^9}, {3.817466254017514*^9, 3.817466312191326*^9},
3.817473469655705*^9, {3.817616070193791*^9, 3.8176160704001007`*^9}, {
3.817616106402553*^9, 3.817616138705028*^9}, {3.81761645464653*^9,
3.817616521373534*^9}, {3.817618123606201*^9, 3.817618152793016*^9}},
3.817616521373534*^9}, {3.817618123606201*^9, 3.817618152793016*^9}, {
3.8180094803954144`*^9, 3.818009506714346*^9}, {3.8180099934634867`*^9,
3.818010051334861*^9}},
CellLabel->
"In[1333]:=",ExpressionUUID->"ca8158ca-2cc6-4c2b-92b0-ae9728cb14cd"],
"In[138]:=",ExpressionUUID->"ca8158ca-2cc6-4c2b-92b0-ae9728cb14cd"],

Cell[BoxData[
RowBox[{
FractionBox["1",
RowBox[{"12", " ",
SuperscriptBox["jMax", "4"]}]],
RowBox[{"(",
RowBox[{
RowBox[{"-",
RowBox[{
FractionBox["1",
RowBox[{"12", " ",
SuperscriptBox["jMax", "4"]}]],
RowBox[{"(",
RowBox[{
RowBox[{"-", "3"}], " ",
SuperscriptBox["a0", "4"]}], "+",
RowBox[{"3", " ",
SuperscriptBox["af", "4"]}], "+",
RowBox[{"8", " ",
SuperscriptBox["a0", "3"], " ", "aMax"}], "-",
RowBox[{"8", " ",
SuperscriptBox["af", "3"], " ", "aMax"}], "-",
RowBox[{"24", " ", "a0", " ", "aMax", " ", "jMax", " ", "v0"}], "-",
RowBox[{"6", " ",
SuperscriptBox["a0", "2"], " ",
RowBox[{"(",
RowBox[{
SuperscriptBox["aMax", "2"], "-",
RowBox[{"2", " ", "jMax", " ", "v0"}]}], ")"}]}], "-",
RowBox[{"24", " ", "af", " ", "aMax", " ", "jMax", " ", "vf"}], "+",
RowBox[{"6", " ",
SuperscriptBox["af", "2"], " ",
RowBox[{"(",
RowBox[{
SuperscriptBox["aMax", "2"], "+",
RowBox[{"2", " ", "jMax", " ", "vf"}]}], ")"}]}], "+",
RowBox[{"12", " ", "jMax", " ",
RowBox[{"(",
RowBox[{
RowBox[{"2", " ", "aMax", " ", "jMax", " ",
RowBox[{"(",
RowBox[{"p0", "-", "pf"}], ")"}]}], "+",
RowBox[{
RowBox[{"-", "3"}], " ",
SuperscriptBox["a0", "4"]}], "+",
RowBox[{"3", " ",
SuperscriptBox["af", "4"]}], "+",
RowBox[{"8", " ",
SuperscriptBox["a0", "3"], " ", "aMax"}], "-",
RowBox[{"8", " ",
SuperscriptBox["af", "3"], " ", "aMax"}], "+",
RowBox[{"24", " ", "a0", " ", "aMax", " ", "jMax", " ", "v0"}], "+",
RowBox[{"6", " ",
SuperscriptBox["a0", "2"], " ",
RowBox[{"(",
RowBox[{
SuperscriptBox["aMax", "2"], " ",
RowBox[{"(",
RowBox[{"v0", "+", "vf"}], ")"}]}], "+",
RowBox[{"jMax", " ",
RowBox[{"(",
RowBox[{
RowBox[{"-",
SuperscriptBox["v0", "2"]}], "+",
SuperscriptBox["vf", "2"]}], ")"}]}]}], ")"}]}]}],
")"}]}]], "Output",
SuperscriptBox["aMax", "2"], "-",
RowBox[{"2", " ", "jMax", " ", "v0"}]}], ")"}]}], "-",
RowBox[{"24", " ", "af", " ", "aMax", " ", "jMax", " ", "vf"}], "+",
RowBox[{"6", " ",
SuperscriptBox["af", "2"], " ",
RowBox[{"(",
RowBox[{
SuperscriptBox["aMax", "2"], "+",
RowBox[{"2", " ", "jMax", " ", "vf"}]}], ")"}]}], "+",
RowBox[{"12", " ", "jMax", " ",
RowBox[{"(",
RowBox[{
RowBox[{"2", " ", "aMax", " ", "jMax", " ",
RowBox[{"(",
RowBox[{"p0", "-", "pf"}], ")"}]}], "+",
RowBox[{
SuperscriptBox["aMax", "2"], " ",
RowBox[{"(",
RowBox[{"v0", "+", "vf"}], ")"}]}], "+",
RowBox[{"jMax", " ",
RowBox[{"(",
RowBox[{
RowBox[{"-",
SuperscriptBox["v0", "2"]}], "+",
SuperscriptBox["vf", "2"]}], ")"}]}]}], ")"}]}]}],
")"}]}]}]], "Output",
CellChangeTimes->{
3.817464224080229*^9, {3.8174646757284613`*^9, 3.817464735056591*^9}, {
3.817464772067339*^9, 3.817464808446479*^9}, {3.8174649500267267`*^9,
Expand All @@ -610,9 +611,11 @@ Cell[BoxData[
3.817466257823419*^9, 3.817466313043701*^9}, {3.8174734582456903`*^9,
3.8174734702907677`*^9}, {3.817616067838916*^9, 3.817616070838888*^9}, {
3.817616106894815*^9, 3.817616139189769*^9}, {3.817616459558393*^9,
3.817616521866693*^9}, {3.817618128281868*^9, 3.817618153255725*^9}},
3.817616521866693*^9}, {3.817618128281868*^9, 3.817618153255725*^9}, {
3.818009482010332*^9, 3.818009507179964*^9}, {3.8180100256655703`*^9,
3.818010051730009*^9}},
CellLabel->
"Out[1336]=",ExpressionUUID->"5041e5a2-bdd6-4cbc-947c-5a048a6fee8c"]
"Out[141]=",ExpressionUUID->"f542bf2e-909f-4382-b877-d01cf719c719"]
}, Open ]],

Cell[CellGroupData[{
Expand All @@ -628,7 +631,7 @@ Cell[BoxData[{
RowBox[{"Simplify", "[",
RowBox[{"tmpReplaced", "[",
RowBox[{"[",
RowBox[{"7", ",", "2"}], "]"}], "]"}], "]"}]}], "\[IndentingNewLine]",
RowBox[{"6", ",", "2"}], "]"}], "]"}], "]"}]}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"ToString", "[",
RowBox[{"tmp", ",", "CForm"}], "]"}], ";"}], "\[IndentingNewLine]",
Expand All @@ -640,24 +643,41 @@ Cell[BoxData[{
3.817465701685096*^9}, {3.817473479559062*^9, 3.8174734800856733`*^9}, {
3.817473533672271*^9, 3.817473533751308*^9}, {3.8174742089250727`*^9,
3.817474210971596*^9}, {3.817616151988146*^9, 3.817616183627185*^9}, {
3.817618162951096*^9, 3.8176181782700644`*^9}},
3.817618162951096*^9, 3.8176181782700644`*^9}, {3.818009517172654*^9,
3.818009538091856*^9}, {3.8180100608100224`*^9, 3.81801007299201*^9}},
CellLabel->
"In[1363]:=",ExpressionUUID->"02ddacce-ea40-40b5-bbc9-e4e0e8a8500b"],
"In[164]:=",ExpressionUUID->"02ddacce-ea40-40b5-bbc9-e4e0e8a8500b"],

Cell[BoxData[
RowBox[{
RowBox[{"-",
FractionBox["af", "jMax"]}], "+", "t"}]], "Output",
RowBox[{"-",
FractionBox[
RowBox[{
SuperscriptBox["a0", "2"], "-",
SuperscriptBox["af", "2"], "+",
RowBox[{"4", " ", "a0", " ", "jMax", " ", "t"}], "+",
RowBox[{"2", " ",
RowBox[{"(",
RowBox[{
SuperscriptBox["aMax", "2"], "+",
RowBox[{"jMax", " ",
RowBox[{"(",
RowBox[{
RowBox[{"jMax", " ",
SuperscriptBox["t", "2"]}], "+", "v0", "-", "vf"}], ")"}]}]}],
")"}]}]}],
RowBox[{"2", " ", "aMax", " ", "jMax"}]]}]], "Output",
CellChangeTimes->{{3.817464289219615*^9, 3.817464337401743*^9}, {
3.817464881225731*^9, 3.817464920476918*^9}, {3.817464988238978*^9,
3.81746500768358*^9}, {3.81746540086765*^9, 3.817465413431589*^9}, {
3.817465461523279*^9, 3.817465497766103*^9}, {3.817465655693458*^9,
3.817465703495722*^9}, 3.817466232751576*^9, 3.817473410326756*^9, {
3.817473459931615*^9, 3.8174734881471148`*^9}, 3.81747354157975*^9, {
3.81747420945721*^9, 3.8174742112747707`*^9}, {3.8176161501097183`*^9,
3.817616185177759*^9}, {3.817618163459977*^9, 3.817618178516623*^9}},
3.817616185177759*^9}, {3.817618163459977*^9, 3.817618178516623*^9}, {
3.818009517630191*^9, 3.8180095385414467`*^9}, {3.8180100612506733`*^9,
3.8180100732781763`*^9}},
CellLabel->
"Out[1364]=",ExpressionUUID->"b12da6aa-4361-4dff-b279-45c07bfe4387"]
"Out[165]=",ExpressionUUID->"77115233-e544-4ee6-a2f9-7f5622bcfff6"]
}, Open ]],

Cell[CellGroupData[{
Expand Down Expand Up @@ -795,30 +815,30 @@ CellTagsIndex->{}
(*NotebookFileOutline
Notebook[{
Cell[558, 20, 250, 8, 81, "Text",ExpressionUUID->"f0efc888-777d-4c66-b8e6-2db88daacc31"],
Cell[811, 30, 7173, 225, 875, "Input",ExpressionUUID->"d346b209-9dd3-47cf-8f1e-f04bc3bf980a"],
Cell[7987, 257, 5622, 133, 325, "Input",ExpressionUUID->"ccba75f7-892e-4119-93ec-41811b2e330e"],
Cell[13612, 392, 1221, 23, 242, "Text",ExpressionUUID->"e82cb2e9-b4f4-4bc2-9a74-547157d3c4dc"],
Cell[14836, 417, 249, 7, 58, "Text",ExpressionUUID->"2bb13fa1-f9ef-4a5d-ac3b-101e020ad421"],
Cell[811, 30, 7190, 224, 875, "Input",ExpressionUUID->"d346b209-9dd3-47cf-8f1e-f04bc3bf980a"],
Cell[8004, 256, 5618, 132, 325, "Input",ExpressionUUID->"ccba75f7-892e-4119-93ec-41811b2e330e"],
Cell[13625, 390, 1221, 23, 242, "Text",ExpressionUUID->"e82cb2e9-b4f4-4bc2-9a74-547157d3c4dc"],
Cell[14849, 415, 249, 7, 58, "Text",ExpressionUUID->"2bb13fa1-f9ef-4a5d-ac3b-101e020ad421"],
Cell[CellGroupData[{
Cell[15110, 428, 1796, 38, 115, "Input",ExpressionUUID->"f20278fb-5b15-4c9c-bd38-9f1d3931e2f3"],
Cell[16909, 468, 942, 13, 34, "Output",ExpressionUUID->"d3d06386-dc01-4a4e-ab6e-96f532c5d53c"],
Cell[17854, 483, 973, 15, 54, "Output",ExpressionUUID->"44ab03ef-9010-4438-9728-59e5a07bb808"]
Cell[15123, 426, 1796, 38, 115, "Input",ExpressionUUID->"f20278fb-5b15-4c9c-bd38-9f1d3931e2f3"],
Cell[16922, 466, 942, 13, 34, "Output",ExpressionUUID->"d3d06386-dc01-4a4e-ab6e-96f532c5d53c"],
Cell[17867, 481, 973, 15, 54, "Output",ExpressionUUID->"44ab03ef-9010-4438-9728-59e5a07bb808"]
}, Open ]],
Cell[18842, 501, 166, 3, 35, "Text",ExpressionUUID->"8d7af2fe-a47e-4ef4-b28a-db0dc764dd0f"],
Cell[18855, 499, 166, 3, 35, "Text",ExpressionUUID->"8d7af2fe-a47e-4ef4-b28a-db0dc764dd0f"],
Cell[CellGroupData[{
Cell[19033, 508, 1977, 47, 159, "Input",ExpressionUUID->"ca8158ca-2cc6-4c2b-92b0-ae9728cb14cd"],
Cell[21013, 557, 2224, 57, 102, "Output",ExpressionUUID->"5041e5a2-bdd6-4cbc-947c-5a048a6fee8c"]
Cell[19046, 506, 2080, 49, 159, "Input",ExpressionUUID->"ca8158ca-2cc6-4c2b-92b0-ae9728cb14cd"],
Cell[21129, 557, 2388, 60, 125, "Output",ExpressionUUID->"f542bf2e-909f-4382-b877-d01cf719c719"]
}, Open ]],
Cell[CellGroupData[{
Cell[23274, 619, 1187, 25, 94, "Input",ExpressionUUID->"02ddacce-ea40-40b5-bbc9-e4e0e8a8500b"],
Cell[24464, 646, 754, 13, 54, "Output",ExpressionUUID->"b12da6aa-4361-4dff-b279-45c07bfe4387"]
Cell[23554, 622, 1282, 26, 94, "Input",ExpressionUUID->"02ddacce-ea40-40b5-bbc9-e4e0e8a8500b"],
Cell[24839, 650, 1304, 29, 58, "Output",ExpressionUUID->"77115233-e544-4ee6-a2f9-7f5622bcfff6"]
}, Open ]],
Cell[CellGroupData[{
Cell[25255, 664, 1671, 37, 94, "Input",ExpressionUUID->"bbf1a9de-0221-4992-b9c6-d1f769287d62"],
Cell[26929, 703, 2462, 53, 98, "Output",ExpressionUUID->"105051c1-dd97-49a1-a1ea-1856a027794e"],
Cell[29394, 758, 829, 13, 34, "Output",ExpressionUUID->"ee561e7d-9711-4560-80d9-d30648c0977e"]
Cell[26180, 684, 1671, 37, 94, "Input",ExpressionUUID->"bbf1a9de-0221-4992-b9c6-d1f769287d62"],
Cell[27854, 723, 2462, 53, 98, "Output",ExpressionUUID->"105051c1-dd97-49a1-a1ea-1856a027794e"],
Cell[30319, 778, 829, 13, 34, "Output",ExpressionUUID->"ee561e7d-9711-4560-80d9-d30648c0977e"]
}, Open ]],
Cell[30238, 774, 154, 3, 30, "Input",ExpressionUUID->"f0b1e6e6-f499-4d6a-bda7-66c7bc16539e"]
Cell[31163, 794, 154, 3, 30, "Input",ExpressionUUID->"f0b1e6e6-f499-4d6a-bda7-66c7bc16539e"]
}
]
*)
Expand Down
2 changes: 1 addition & 1 deletion src/python.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ PYBIND11_MODULE(_ruckig, m) {
given a target waypoint with position, velocity, and acceleration, starting from any initial state \
limited by velocity, acceleration, and jerk constraints.";

constexpr size_t DOFs {3};
constexpr size_t DOFs {1};

py::class_<InputParameter<DOFs>>(m, "InputParameter")
.def(py::init<>())
Expand Down
Loading

0 comments on commit 5533d34

Please sign in to comment.