├── .github └── workflows │ ├── cd.yml │ └── ci.yml ├── .gitignore ├── CHANGELOG.rst ├── CITATION.cff ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake └── ruckig-config.cmake.in ├── doc ├── benchmark.png ├── example_profile.png └── ruckig_toppra_example.png ├── examples ├── 01_position.cpp ├── 01_position.py ├── 01_trajectory.pdf ├── 02_position_offline.cpp ├── 02_position_offline.py ├── 02_trajectory.pdf ├── 03_trajectory.pdf ├── 03_waypoints.cpp ├── 03_waypoints.py ├── 04_trajectory.pdf ├── 04_waypoints_online.cpp ├── 04_waypoints_online.py ├── 05_trajectory.pdf ├── 05_velocity.cpp ├── 05_velocity.py ├── 06_stop.cpp ├── 06_stop.py ├── 06_trajectory.pdf ├── 07_minimum_duration.cpp ├── 07_minimum_duration.py ├── 07_trajectory.pdf ├── 08_per_section_minimum_duration.cpp ├── 08_per_section_minimum_duration.py ├── 08_trajectory.pdf ├── 09_dynamic_dofs.cpp ├── 09_dynamic_dofs.py ├── 09_trajectory.pdf ├── 10_dynamic_dofs_waypoints.cpp ├── 10_dynamic_dofs_waypoints.py ├── 10_trajectory.pdf ├── 11_eigen_vector_type.cpp ├── 11_eigen_vector_type.py ├── 11_trajectory.pdf ├── 12_custom_vector_type.cpp ├── 12_custom_vector_type.py ├── 12_trajectory.pdf ├── 13_custom_vector_type_dynamic_dofs.cpp ├── 13_custom_vector_type_dynamic_dofs.py ├── 13_trajectory.pdf ├── 14_tracking.cpp ├── 14_tracking.py ├── 14_trajectory.pdf ├── CMakeLists-directory.txt ├── CMakeLists-installed.txt └── plotter.py ├── include └── ruckig │ ├── block.hpp │ ├── brake.hpp │ ├── calculator.hpp │ ├── calculator_cloud.hpp │ ├── calculator_target.hpp │ ├── error.hpp │ ├── input_parameter.hpp │ ├── output_parameter.hpp │ ├── position.hpp │ ├── profile.hpp │ ├── result.hpp │ ├── roots.hpp │ ├── ruckig.hpp │ ├── trajectory.hpp │ ├── utils.hpp │ └── velocity.hpp ├── package.xml ├── pyproject.toml ├── src └── ruckig │ ├── brake.cpp │ ├── cloud_client.cpp │ ├── position_first_step1.cpp │ ├── position_first_step2.cpp │ ├── position_second_step1.cpp │ ├── position_second_step2.cpp │ ├── position_third_step1.cpp │ ├── position_third_step2.cpp │ ├── python.cpp │ ├── velocity_second_step1.cpp │ ├── velocity_second_step2.cpp │ ├── velocity_third_step1.cpp │ └── velocity_third_step2.cpp ├── test ├── benchmark_target.cpp ├── randomizer.hpp └── test_target.cpp └── third_party ├── doctest ├── LICENSE └── doctest.h ├── httplib ├── LICENSE └── httplib.h └── nlohmann ├── LICENSE └── json.hpp /.github/workflows/cd.yml: -------------------------------------------------------------------------------- 1 | name: CD 2 | 3 | on: 4 | workflow_dispatch: 5 | release: 6 | types: [released] 7 | 8 | jobs: 9 | build-wheels: 10 | # if: ${{ false }} 11 | runs-on: ${{ matrix.os }} 12 | strategy: 13 | matrix: 14 | os: [ubuntu-latest, windows-latest, macos-13, macos-14] 15 | 16 | steps: 17 | - uses: actions/checkout@v4 18 | 19 | - name: Build wheels 20 | uses: pypa/cibuildwheel@v2.23.3 21 | env: 22 | CIBW_SKIP: pp* # Skip PyPy 23 | MACOSX_DEPLOYMENT_TARGET: "10.14" 24 | 25 | - uses: actions/upload-artifact@v4 26 | with: 27 | name: wheels-${{ matrix.os }}-${{ strategy.job-index }} 28 | path: ./wheelhouse/*.whl 29 | 30 | 31 | make-source-dist: 32 | runs-on: ubuntu-latest 33 | 34 | steps: 35 | - uses: actions/checkout@v4 36 | 37 | - name: Build SDist 38 | run: pipx run build --sdist 39 | 40 | - uses: actions/upload-artifact@v4 41 | with: 42 | path: dist/*.tar.gz 43 | 44 | 45 | upload: 46 | # if: ${{ false }} 47 | needs: [build-wheels, make-source-dist] 48 | runs-on: ubuntu-latest 49 | permissions: 50 | id-token: write 51 | 52 | steps: 53 | - uses: actions/download-artifact@v4 54 | with: 55 | name: artifact 56 | path: dist 57 | 58 | - name: Upload to PyPI 59 | uses: pypa/gh-action-pypi-publish@release/v1 60 | -------------------------------------------------------------------------------- /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | build-and-test: 7 | runs-on: ${{ matrix.os }} 8 | 9 | strategy: 10 | fail-fast: false 11 | matrix: 12 | os: [ubuntu-latest, macos-latest, windows-latest] 13 | cmake_flags: ["", " -DBUILD_CLOUD_CLIENT=OFF "] 14 | 15 | steps: 16 | - uses: actions/checkout@v4 17 | 18 | - name: Get cross-platform /dev/null for hidden output 19 | run: echo "HIDDEN=$(python3 -c "import os; print(os.devnull)")" >> $GITHUB_ENV 20 | 21 | - name: Install valgrind 22 | if: matrix.os == 'ubuntu-latest' 23 | run: sudo apt-get update && sudo apt-get install -y valgrind 24 | 25 | - name: Configure and make 26 | run: | 27 | cmake -B build -DCMAKE_BUILD_TYPE=Release -DBUILD_EXAMPLES=ON -DBUILD_BENCHMARK=ON ${{ matrix.cmake_flags }} -DCMAKE_RUNTIME_OUTPUT_DIRECTORY_RELEASE=. -DCMAKE_LIBRARY_OUTPUT_DIRECTORY_RELEASE=. 28 | cmake --build build --config Release -j2 29 | 30 | - name: Run C++ Examples 31 | working-directory: build 32 | run: | 33 | ./example-01_position > "$HIDDEN" 34 | ./example-02_position_offline > "$HIDDEN" 35 | ./example-05_velocity > "$HIDDEN" 36 | ./example-06_stop > "$HIDDEN" 37 | ./example-07_minimum_duration > "$HIDDEN" 38 | ./example-09_dynamic_dofs > "$HIDDEN" 39 | 40 | - name: Test 41 | if: matrix.os == 'ubuntu-latest' 42 | run: | 43 | ./build/test-target 5000000 44 | env: 45 | CTEST_OUTPUT_ON_FAILURE: 1 46 | 47 | - name: Memory Test 48 | if: matrix.os == 'ubuntu-latest' 49 | run: | 50 | ctest --test-dir ./build -T memcheck 51 | 52 | 53 | build-python-package: 54 | runs-on: ${{ matrix.os }} 55 | 56 | strategy: 57 | fail-fast: false 58 | matrix: 59 | os: [ubuntu-latest, macos-latest, windows-latest] 60 | 61 | steps: 62 | - uses: actions/checkout@v4 63 | 64 | - uses: actions/setup-python@v5 65 | with: 66 | python-version: '3.10' 67 | 68 | - name: Get cross-platform /dev/null for hidden output 69 | run: echo "HIDDEN=$(python -c "import os; print(os.devnull)")" >> $GITHUB_ENV 70 | 71 | - name: Install ruckig 72 | run: pip install . 73 | 74 | - name: Run Python Examples 75 | run: | 76 | python ./examples/01_position.py > "$HIDDEN" 77 | python ./examples/02_position_offline.py > "$HIDDEN" 78 | python ./examples/05_velocity.py > "$HIDDEN" 79 | python ./examples/06_stop.py > "$HIDDEN" 80 | python ./examples/07_minimum_duration.py > "$HIDDEN" 81 | 82 | 83 | build-debian-package: 84 | runs-on: ubuntu-latest 85 | 86 | steps: 87 | - uses: actions/checkout@v4 88 | 89 | - name: Configure and make 90 | run: | 91 | cmake -B build 92 | cmake --build build -j2 -- package 93 | 94 | - name: Install package 95 | working-directory: build 96 | run: sudo apt-get install ./ruckig-*.deb 97 | 98 | - name: Test building examples 99 | working-directory: examples 100 | run: | 101 | mv CMakeLists-installed.txt CMakeLists.txt 102 | cmake -B build 103 | cmake --build build -j2 104 | 105 | 106 | lint-python: 107 | runs-on: ubuntu-latest 108 | 109 | steps: 110 | - uses: actions/checkout@v4 111 | 112 | - name: Lint Python 113 | run: | 114 | python3 -m pip install ruff 115 | ruff check examples test --preview 116 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | .DS_Store 3 | 4 | .ruff_cache 5 | .mypy_cache 6 | *.pyc 7 | __pycache__ 8 | node_modules 9 | .parcel-cache 10 | *.egg-info 11 | 12 | .env 13 | build 14 | dist 15 | doc/html 16 | local 17 | patch-* 18 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ruckig 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | * [ci] don't check python for c++11 8 | * [ci] move gcc-5 to ubuntu 20.04 9 | * bump version 10 | * add example for per section minimum duration 11 | * Budan's theorem 12 | * use valid profile iterator in step1 13 | * add jerk-minimizing profiles in step2 14 | * fix c++11 patch 15 | * fix c++11 patch 16 | * fix patch c++11 17 | * c++11 fix inplace patching 18 | * improve support for c++11 19 | * update pybind11 in ci 20 | * fix optional in tests with c++11 patch 21 | * update ci checkout version 22 | * fix msvc warning in online calculator 23 | * fix msvc compiler warnings 24 | * expose calculators 25 | * bump version 26 | * performance improvement in step2 vel 27 | * Contributors: pantor 28 | 29 | 0.8.4 (2022-09-13) 30 | ------------------ 31 | * robustify step2 vel uddu 32 | * fix readme tracking example 33 | * add tracking interface to readme 34 | * fix brake duration reset 35 | * fix brake when current acceleration is on limit 36 | * improve tracking example 37 | * fix pyproject.toml 38 | * clean tracking example 39 | * clean examples 40 | * add pyproject file 41 | * add DOFs to traj serialization 42 | * nlohmann/json based serialization for trajectory 43 | * point out Eigen 3.4 or later 44 | * add custom vector types to readme 45 | * disable vector types example with online client 46 | * include deque 47 | * add examples for custom vector types 48 | * dont support custom data types with online client 49 | * add tests and fixes for custom vector type 50 | * use template specalization without using 51 | * custom vector type as template template parameter 52 | * clean error_at_max_duration, add StandardVector 53 | * clean licenses 54 | * update header dependencies 55 | * clean comparison benchmarks 56 | * extend phase synchronization to velocity control 57 | * move src to src/ruckig 58 | * clean gitignore 59 | * remove -Werror 60 | * Contributors: pantor 61 | 62 | 0.7.1 (2022-07-10) 63 | ------------------ 64 | * bump to 0.7.1 65 | * fix python 3.10 deployment 66 | * Merge branch 'master' of github.com:pantor/ruckig 67 | * bump to 0.7.0 68 | * allow user to force new computation for the same input (`#132 `_) 69 | * allow user to force computation again 70 | Otherwise sending the same target with non-zero min_duration 71 | multiple times in a control loop will not trigger a new trajectory. 72 | * rename to reset 73 | Co-authored-by: pantor 74 | * profile precision as constant 75 | * clean notebooks 76 | * fix c++11 std::clamp 77 | * use steady_clock, minor code cleaning of roots.hpp 78 | * fix numeric for time sync with discrete durations 79 | * fix independent min duration with brake trajectory 80 | * clean header includes 81 | * improve stability of velocity control 82 | * msvc warnings 83 | * fix msvc warnings 84 | * static cast for std::div 85 | * Merge branch 'master' of github.com:pantor/ruckig 86 | * fix warnings, limiting_dof to std::optional 87 | * doc: fixed used-by entry (`#131 `_) 88 | * move roots into ruckig namespace 89 | * refactor tests and benchmarks 90 | * step2 vel catch root at boundary 91 | * update benchmark plot 92 | * add algorithm header, use c++ array 93 | * make delta_time non const 94 | * add smoothing of non-limiting dofs 95 | * const degrees_of_freedom 96 | * print error messages from online api 97 | * check that current_input is initialized 98 | * Contributors: Matthias Seehauser, Michael Görner, pantor 99 | 100 | 0.6.5 (2022-03-06) 101 | ------------------ 102 | * revert to manylinux2010_x86_64 103 | * bump to v0.6.5 104 | * fix pypi build from source 105 | * remove 9_trajectory.png 106 | * use .pdf for example trajectories 107 | * numerical stability in velocity control 108 | * fix for zero-to-zero traj with velocity control 109 | * invalidate discrete duration 110 | * fix numerical instability in acc0 111 | * update to nlohmann/json v3.10.5 112 | * bump to 0.6.4 113 | * clarify Online API in examples and Readme 114 | * fix example docs 115 | * fix ci 116 | * build waypoints example only for online client 117 | * add joint example for dynamic dofs and waypoints 118 | * fix capacity / actual waypoints mismatch 119 | * disable ros cd 120 | * retry 121 | * retry 122 | * retry 123 | * debug publish ros 124 | * Contributors: pantor 125 | 126 | 0.6.3 (2022-01-21) 127 | ------------------ 128 | * bump to v0.6.3 129 | * activaten open_pr for bloom release 130 | * publish ros on release 131 | * test bloom ci 132 | * add bloom release 133 | * several perf optimizations, step2 stability 134 | * clear up waypoints in readme 135 | * fix time sync for discrete durations, rename step1 136 | * Contributors: pantor 137 | 138 | 0.6.0 (2021-12-06) 139 | ------------------ 140 | * fix python upload 141 | * bump version to 0.6.0 142 | * filter_intermediate_positions 143 | * add braces to if function 144 | * fix error in step 2 145 | * remove filter positions 146 | * remote api for intermediate waypoints 147 | * fix trajectories with zero duration 148 | * use integrated instead target values after traj 149 | * use back() instead of explicit number 150 | * ci build matrix 151 | * BUILD_ONLINE_CLIENT in python package 152 | * add brake in online calculator 153 | * fix ci online-calculator 154 | * auto ci name 155 | * add online calculator for intermediate waypoints 156 | * add httplib and build option 157 | * create third_party directory 158 | * update demo notebook 159 | * update notebook demo 160 | * add jupyter demo notebook 161 | * change brief description of calculator 162 | * expose internal 163 | * add note to ruckig pro examples 164 | * clear up section term 165 | * clean brake class 166 | * refactor integrate to utils 167 | * prepare accel phase 168 | * use dynamic dofs const 169 | * improve input validation 170 | * clean up CD 171 | * Contributors: pantor 172 | 173 | 0.5.0 (2021-11-14) 174 | ------------------ 175 | * debug publish 176 | * publish pypi package on released release 177 | * bump version 178 | * add hint for Step 1 None 179 | * optimize block class 180 | * improve readme 181 | * per_section_limits in readme 182 | * add per_section_limits 183 | * fix c+11 patch 184 | * fix per dof synchronization 185 | * split CMakeLists examples 186 | * fix per dof synchronization with none 187 | * separate trajectory and calculator class 188 | * fix windows build 189 | * code cleaning 190 | * intermediate waypoints readme 191 | * fix number of waypoints 192 | * avoid pow completely 193 | * update maintainer 194 | * simplify readme 195 | * use brake profile class 196 | * fix finished trajectory for disabled dofs 197 | * minor code cleaning 198 | * Merge branch 'master' of github.com:pantor/ruckig 199 | * add to_string for output parameters (`#77 `_) 200 | * add ref to vel add_profile 201 | * positional limits 202 | * min limits for intermediate positions 203 | * extend phase synchronization 204 | * performance improvements 205 | * add section to output in readme 206 | * pass_to_input, did_section_change 207 | * fix nullopt with c++11 patch 208 | * fix nullopt in c++11 patch 209 | * fix c++11 210 | * per dof control interface and synchronization `#53 `_ 211 | * add section index to output 212 | * Notes about Intermediate Waypoints 213 | * interrupt calculation duration in microseconds 214 | * add ruckig pro examples 215 | * add ruckig toppra comparison 216 | * improve readme and examples 217 | * introduce Ruckig Pro 218 | * remove generate docs ci 219 | * link docs to ruckig.com 220 | * add examples doc pages 221 | * fix example names 222 | * add more examples 223 | * Instantaneous Motion Generation 224 | * add calculation interruption 225 | * add doxyfile again 226 | * step1: numeric stability 227 | * Contributors: Lars Berscheid, lpdon, pantor 228 | 229 | 0.4.0 (2021-08-23) 230 | ------------------ 231 | * update version to 0.4 232 | * code cleaning 233 | * add was_calculation_interrupted 234 | * step 1: performance optimizations 235 | * interrupt_calculation_duration 236 | * Add CITATION.cff file (`#63 `_) 237 | * add CITATION.cff 238 | * CITATOION wip 239 | * fix cite 240 | * Update README.md 241 | * update to doctest 2.4.6 242 | * code cleaning 243 | * performance optimizations 244 | * step 2: performance optimization 245 | * step 2: performance optimization 246 | * performance optimization: step 1 247 | * performance optimization: positive set of roots 248 | * performance optimization in step1 249 | * code cleaning 250 | * set find_package reflexxes to quiet 251 | * remove BSD license text, why was it there anyway? 252 | * clean plot trajectory 253 | * add printing intermediate_positions 254 | * code cleaning 255 | * add degrees_of_freedom to python trajectory 256 | * rename interface to control_interface 257 | * set_limits for ACC1, code cleaning 258 | * improve numeric stability 259 | * in vel interface, change acc threshold 260 | * code cleanup 261 | * add DynamicDOFs constant 262 | * numerical stability of velocity interface 263 | * improve numerical stability 264 | * fix variable name redeclaration 265 | * fix jerk violation in step2, some optimizations 266 | * clean up check methods of profile 267 | * fix min_velocity with phas e synchronization 268 | * fix phase synchronization in python 269 | * improve numerical stability for high jerks 270 | * fix newton step in acc0/acc1 for t=0 271 | * clean up plot_trajectory 272 | * validate for isnan 273 | * fix python path in examples 274 | * fix position extrema for dynamic number of dofs 275 | * Added python example for non-realtime context (`#43 `_) 276 | * added python example for non-realtime context 277 | * fixed ci workflow 278 | * need to reset t_start 279 | * rename 280 | * change example 281 | Co-authored-by: Max Dhom 282 | Co-authored-by: Lars Berscheid 283 | * Dynamic Dofs (`#47 `_) 284 | * vectorize 285 | * use vector for dof==0 286 | * add tests and checks 287 | * include vector 288 | * default dofs = 0, fix reflexxes 289 | * redo default dofs template parameter 290 | * add readme 291 | * improve readme 292 | * fix grammar 293 | * add tests for invalid input 294 | * add offline trajectory generation 295 | * add paren to get time at position 296 | * add get_first_time_at_position method 297 | * Contributors: Lars Berscheid, Mathis, Max Dhom, pantor 298 | 299 | 0.3.3 (2021-06-25) 300 | ------------------ 301 | * Merge branch 'master' of github.com:pantor/ruckig 302 | * version 0.3.3 303 | * Set CMAKE_OUTPUT_DIRECTORY for windows build (`#41 `_) 304 | * check windows 305 | * add library 306 | * add debug log 307 | * try to fix 308 | * try to fix2 309 | * try to fix 3 310 | * try to fix 4 311 | * fix 5 312 | * rest test number 313 | * fix setup.py 314 | * remove log 315 | * hard-code build directory of python library 316 | * fix windows packge, version 0.3.2 317 | * pre-compiled packages for windows on pypi 318 | * Contributors: Lars Berscheid, pantor 319 | 320 | 0.3.1 (2021-06-24) 321 | ------------------ 322 | * set version 0.3.1 323 | * add manifest.in 324 | * double newton step in step2 vel udud 325 | * Fix windows python package (`#38 `_) 326 | * fix windows 327 | * update benchmark figure 328 | * c++11 dont treat warnings as errors 329 | * fix three step 330 | * performance improvements 331 | * vectorize dof 332 | * Fix Patch for C++11 (`#36 `_) 333 | * add ci for c++11 334 | * remove maybe_unused 335 | * patch in-place 336 | * fix c++11 337 | * replace make_unique 338 | * find error in ci 339 | * try to fix gcc-5 340 | * dont build python 341 | * dont patch cpp files 342 | * deactivate cmake flags in patch script 343 | * test python example 344 | * add C++11 patch in readme 345 | * add patch script for C++11 346 | * Contributors: Lars Berscheid, pantor 347 | 348 | 0.3.0 (2021-06-16) 349 | ------------------ 350 | * update version number 351 | * add python at_time comment for doxygen 352 | * check for v_min, fix directional tests 353 | * python return at_time method 354 | * fix max target acceleration 355 | * fix and test extremal positions 356 | * synchronize function to trajectory class 357 | * fix max target acceleration when min_velocity 358 | * clean up docs 359 | * fixed bug in check_position_extremum (`#33 `_) 360 | * fixed copy & paste error in Trajectory::is_phase_synchronizable 361 | * fixed obvious copy & paste error in check_position_extremum 362 | * fixed copy & paste error in Trajectory::is_phase_synchronizable (`#32 `_) 363 | * fix negative, near zero pd cases 364 | * Update README.md 365 | * Update README.md 366 | * Update README.md 367 | * check limits with equal sign for numeric stability 368 | * copy jerk_signs for phase synchronization 369 | * remove alternative otgs 370 | * add citation and link to paper 371 | * remove alternative otgs from python 372 | * fix numerical issues on time-optimal traj 373 | * Merge branch 'master' of github.com:pantor/ruckig 374 | * fix numeric error for long durations 375 | * Scale to 5e9 random trajectories 376 | * fix numerical issues 377 | * add step through tests 378 | * fix for braking 379 | * double newton step 380 | * fix macos and windows build 381 | * recalculate after error 382 | * fix some numerical issues 383 | * use checkout@v2 in ci 384 | * use ARCHIVE_OUTPUT_NAME for python wrapper 385 | * msvc: warning as errors 386 | * fix msvc compiler warnings 387 | * Update README.md 388 | * Update README.md 389 | * Fix BUILD_PYTHON_MODULE option on Windows/MSVC (`#18 `_) 390 | * fix ci args 391 | * add phase synchronization 392 | * set boundary method 393 | * simplify python example 394 | * Add pip install to Readme 395 | * Contributors: Lars Berscheid, Silvio Traversaro, pantor, stefanbesler 396 | 397 | 0.2.6 (2021-03-29) 398 | ------------------ 399 | * remove env 400 | * fix python cd 401 | * use static library 402 | * test python executable 403 | * fix python package 404 | * python cd: fix windows 405 | * add setup.py, version 0.2.1 406 | * rename python module to ruckig 407 | * generate python classes for multiple dof 408 | * Add a ROS package manifest (`#10 `_) 409 | Enables building Ruckig as a plain CMake package in a Catkin/Colcon workspace. 410 | * fix end of brake trajectory integration 411 | * Include GNU install dirs earlier (`#11 `_) 412 | Otherwise 'CMAKE_INSTALL_INCLUDEDIR' is empty/undefined when it's used to setup ruckig::ruckig's target_include_directories(..). 413 | * readme: some minor typos (`#9 `_) 414 | I happened to notice them. 415 | * privatize trajectory class members 416 | * privatize some class members 417 | * use cmath 418 | * code cleaning 419 | * show enum in docs 420 | * split parameter files, calculate in trajectory 421 | * code cleaning 422 | * add python example 423 | * Move options to API documentation 424 | * Fix Readme Code 425 | * fix undefined output for zero duration 426 | * indicate default values in Readme 427 | * add discrete durations 428 | * Add Windows and macOS build to CI (`#4 `_) 429 | * windows and mac ci 430 | * use cmake action for generator 431 | * fix ci build directory 432 | * run tests only on linux 433 | * test example 434 | * Add support to compile on Windows (`#3 `_) 435 | * Merge pull request `#2 `_ from traversaro/patch-1 436 | Use BUILD_SHARED_LIBS to select if compile C++ library as static or shared 437 | * document examples/CMakeLists.txt 438 | * add ci to PRs 439 | * Use BUILD_SHARED_LIBS to select if compile as static or shared 440 | * Merge pull request `#1 `_ from traversaro/add-install-support 441 | Add support for installation of the C++ library 442 | * Add support for installation of the C++ library 443 | * set correct cmake and doxygen version 444 | * fix more edge cases 445 | * fix some edge cases 446 | * document velocity interface 447 | * added velocity interface 448 | * improve readme 449 | * improve benchmark 450 | * fix finding subdirectory 451 | * check ci 452 | * build python module in ci 453 | * use own set class on stack 454 | * fix synchronization enum, better python support 455 | * add time synchronization parameter 456 | * fix motion finished reset 457 | * fix immediate reaction 458 | * fix more edge cases 459 | * refine min acceleration 460 | * add min_acceleration 461 | * fix some edge cases 462 | * Merge branch 'master' of github.com:pantor/ruckig 463 | * decrease required cmake to 3.10 464 | * fix ci 465 | * introduce trajectory class 466 | * position extrema 467 | * scale tests to 1e9 468 | * several optimizations 469 | * compile with warnings 470 | * step2: code cleaning 471 | * fix numeric edge cases 472 | * move test suite to doctest 473 | * fix cmake as submodule 474 | * Merge branch 'master' of github.com:pantor/ruckig 475 | * fix optional minimum time 476 | * code documentation, more tests 477 | * fix benchmark 478 | * build benchmark in ci 479 | * fix gcc 480 | * remove eigen dep in cmake 481 | * code cleaning 482 | * code cleaning 483 | * add comparison with multiple DoF 484 | * rix ci: braking 485 | * fix interval selection 486 | * clean braking 487 | * fix ci, more tests 488 | * add benchmark, numeric stability 489 | * fix block in step1 490 | * clean root finding 491 | * Increase stability 492 | * improve tests, remove eigen 493 | * code style 494 | * increase test coverage 495 | * add min_velocity 496 | * fix ci 497 | * Step1: Use Newton step 498 | * fix ci 499 | * fix time sync 500 | * update tests 501 | * more tests 502 | * add example 503 | * more tests 504 | * increase number of tests 505 | * simplify equations 506 | * simplify equations 507 | * simplify equations 508 | * add tuple header 509 | * further code cleaning 510 | * remove eigen dependency, code cleaning 511 | * clean brake code 512 | * code cleaning 513 | * code cleaning 514 | * add doxygen 515 | * improve acceleration target, readme 516 | * refine max time deviation to 1e-8 517 | * code cleaning 518 | * improve time sync 519 | * block synchronization 520 | * stability for target acceleration in 1 dof 521 | * update readme for ruckig 522 | * add license 523 | * improve tests 524 | * fix eigen ci folder name 525 | * fix eigen git repository 526 | * fix 1dof vf comparison 527 | * remove complex algorithmic 528 | * code cleaning 529 | * initial commit 530 | * Contributors: G.A. vd. Hoorn, Lars Berscheid, Silvio Traversaro, pantor 531 | -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | # YAML 1.2 2 | --- 3 | cff-version: "1.2.0" 4 | message: "If you use this software, please cite it using these metadata." 5 | type: article 6 | title: "Jerk-limited Real-time Trajectory Generation with Arbitrary Target States" 7 | journal: "Robotics - Science and Systems (2021)" 8 | url: "https://publikationen.bibliothek.kit.edu/1000136018" 9 | doi: "10.15607/RSS.2021.XVII.015" 10 | authors: 11 | - family-names: Berscheid 12 | given-names: Lars 13 | - family-names: "Kröger" 14 | given-names: Torsten 15 | date-released: 2021-07-01 16 | isbn: "978-0-9923747-7-8" 17 | license: MIT 18 | repository-code: "https://github.com/pantor/ruckig" 19 | abstract: "We present Ruckig, an algorithm for Online Trajectory Generation (OTG) respecting third-order constraints and complete kinematic target states. Given any initial state of a system with multiple Degrees of Freedom (DoFs), Ruckig calculates a time-optimal trajectory to an arbitrary target state defined by its position, velocity, and acceleration limited by velocity, acceleration, and jerk constraints. The proposed algorithm and implementation allows three contributions: (1) To the best of our knowledge, we derive the first time-optimal OTG algorithm for arbitrary, multi-dimensional target states, in particular including non-zero target acceleration. (2) This is the first open-source prototype of time-optimal OTG with limited jerk and complete time synchronization for multiple DoFs. (3) Ruckig allows for directional velocity and acceleration limits, enabling robots to better use their dynamical resources. We evaluate the robustness and real-time capability of the proposed algorithm on a test suite with over 1,000,000,000 random trajectories as well as in real-world applications." 20 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.15) 2 | 3 | 4 | project(ruckig VERSION 0.15.3 LANGUAGES CXX) 5 | 6 | 7 | include(GNUInstallDirs) 8 | 9 | 10 | option(BUILD_EXAMPLES "Build example programs" ON) 11 | option(BUILD_PYTHON_MODULE "Build Python wrapper with nanobind" OFF) 12 | option(BUILD_CLOUD_CLIENT "Build cloud client to calculate Ruckig Pro trajectories remotely" ON) 13 | option(BUILD_TESTS "Build tests" ON) 14 | option(BUILD_BENCHMARK "Build benchmark" OFF) 15 | option(BUILD_SHARED_LIBS "Build as shared library" ON) 16 | 17 | if(WIN32 AND BUILD_SHARED_LIBS) 18 | option(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS "On Windows, export all symbols when building a shared library." ON) 19 | endif() 20 | 21 | 22 | add_library(ruckig 23 | src/ruckig/brake.cpp 24 | src/ruckig/position_first_step1.cpp 25 | src/ruckig/position_first_step2.cpp 26 | src/ruckig/position_second_step1.cpp 27 | src/ruckig/position_second_step2.cpp 28 | src/ruckig/position_third_step1.cpp 29 | src/ruckig/position_third_step2.cpp 30 | src/ruckig/velocity_second_step1.cpp 31 | src/ruckig/velocity_second_step2.cpp 32 | src/ruckig/velocity_third_step1.cpp 33 | src/ruckig/velocity_third_step2.cpp 34 | ) 35 | 36 | target_compile_features(ruckig PUBLIC cxx_std_17) 37 | target_include_directories(ruckig PUBLIC 38 | $ 39 | $ 40 | ) 41 | target_link_libraries(ruckig PUBLIC) 42 | 43 | if(MSVC) 44 | target_compile_definitions(ruckig PUBLIC _USE_MATH_DEFINES) 45 | endif() 46 | 47 | if(BUILD_CLOUD_CLIENT) 48 | target_sources(ruckig PRIVATE src/ruckig/cloud_client.cpp) 49 | target_include_directories(ruckig PUBLIC 50 | $ 51 | $ 52 | ) 53 | target_compile_definitions(ruckig PUBLIC WITH_CLOUD_CLIENT) 54 | endif() 55 | 56 | add_library(ruckig::ruckig ALIAS ruckig) 57 | 58 | 59 | if(BUILD_TESTS) 60 | enable_testing() 61 | 62 | add_library(test-dependencies INTERFACE) 63 | target_link_libraries(test-dependencies INTERFACE ruckig) 64 | target_include_directories(test-dependencies INTERFACE third_party) 65 | if(MSVC) 66 | target_compile_options(test-dependencies INTERFACE /W4) 67 | else() 68 | target_compile_options(test-dependencies INTERFACE -Wall -Wextra) 69 | endif() 70 | 71 | add_executable(test-target test/test_target.cpp) 72 | target_link_libraries(test-target PRIVATE test-dependencies) 73 | add_test(NAME test-target COMMAND test-target) 74 | if(NOT MSVC) 75 | include(CTest) 76 | find_program(MEMORYCHECK_COMMAND valgrind) 77 | set(MEMORYCHECK_COMMAND_OPTIONS "--tool=memcheck --leak-check=full --trace-children=yes \ 78 | --track-origins=yes --keep-debuginfo=yes --error-exitcode=100") 79 | endif() 80 | endif() 81 | 82 | 83 | if(BUILD_BENCHMARK) 84 | add_executable(benchmark-target test/benchmark_target.cpp) 85 | 86 | target_link_libraries(benchmark-target PRIVATE ruckig) 87 | endif() 88 | 89 | 90 | if(BUILD_EXAMPLES) 91 | set(EXAMPLES_LIST 01_position 02_position_offline 05_velocity 06_stop 07_minimum_duration 09_dynamic_dofs 12_custom_vector_type 13_custom_vector_type_dynamic_dofs) 92 | if(TARGET Eigen3::Eigen) 93 | list(APPEND EXAMPLES_LIST 11_eigen_vector_type) 94 | endif() 95 | 96 | if(BUILD_CLOUD_CLIENT) 97 | list(APPEND EXAMPLES_LIST 03_waypoints 04_waypoints_online 08_per_section_minimum_duration 10_dynamic_dofs_waypoints) 98 | endif() 99 | 100 | foreach(example IN LISTS EXAMPLES_LIST) 101 | add_executable(example-${example} examples/${example}.cpp) 102 | target_link_libraries(example-${example} PRIVATE ruckig) 103 | endforeach() 104 | endif() 105 | 106 | 107 | if(BUILD_PYTHON_MODULE) 108 | find_package(Python 3.8 REQUIRED COMPONENTS Interpreter Development.Module) 109 | execute_process( 110 | COMMAND "${Python_EXECUTABLE}" -m nanobind --cmake_dir 111 | OUTPUT_STRIP_TRAILING_WHITESPACE OUTPUT_VARIABLE nanobind_ROOT) 112 | find_package(nanobind CONFIG REQUIRED) 113 | 114 | nanobind_add_module(python_ruckig NB_STATIC src/ruckig/python.cpp) 115 | target_compile_features(python_ruckig PUBLIC cxx_std_17) 116 | target_link_libraries(python_ruckig PUBLIC ruckig) 117 | if(BUILD_CLOUD_CLIENT) 118 | target_compile_definitions(python_ruckig PUBLIC WITH_CLOUD_CLIENT) 119 | endif() 120 | 121 | set_target_properties(python_ruckig PROPERTIES OUTPUT_NAME ruckig) 122 | set_target_properties(python_ruckig PROPERTIES ARCHIVE_OUTPUT_NAME python_ruckig) 123 | 124 | install(TARGETS python_ruckig LIBRARY DESTINATION .) 125 | endif() 126 | 127 | 128 | # Add support for installation 129 | include(CMakePackageConfigHelpers) 130 | 131 | # Install headers 132 | install(DIRECTORY include/ruckig DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) 133 | if(BUILD_CLOUD_CLIENT) 134 | install(DIRECTORY third_party/httplib third_party/nlohmann DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/ruckig/third_party) 135 | endif() 136 | 137 | # Install library 138 | install(TARGETS ruckig 139 | EXPORT ${PROJECT_NAME}-targets 140 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 141 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 142 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} 143 | ) 144 | 145 | # Install CMake config files 146 | set(ruckig_INSTALL_CONFIGDIR ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME}) 147 | 148 | install(EXPORT ${PROJECT_NAME}-targets 149 | DESTINATION ${ruckig_INSTALL_CONFIGDIR} 150 | NAMESPACE ruckig:: 151 | ) 152 | 153 | configure_package_config_file(cmake/ruckig-config.cmake.in ruckig-config.cmake 154 | INSTALL_DESTINATION ${ruckig_INSTALL_CONFIGDIR} 155 | ) 156 | 157 | write_basic_package_version_file(ruckig-config-version.cmake 158 | VERSION ${ruckig_VERSION} 159 | COMPATIBILITY AnyNewerVersion 160 | ) 161 | 162 | install(FILES 163 | "${CMAKE_CURRENT_BINARY_DIR}/ruckig-config.cmake" 164 | "${CMAKE_CURRENT_BINARY_DIR}/ruckig-config-version.cmake" 165 | DESTINATION ${ruckig_INSTALL_CONFIGDIR} 166 | ) 167 | 168 | install(FILES 169 | "${CMAKE_CURRENT_SOURCE_DIR}/package.xml" 170 | DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME} 171 | ) 172 | 173 | 174 | # Enable Packaging 175 | set(CPACK_GENERATOR "DEB") 176 | set(CPACK_PACKAGE_CONTACT "Lars Berscheid (lars.berscheid@ruckig.com)") 177 | set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "Motion Generation for Robots and Machines.") 178 | set(CPACK_PACKAGE_VENDOR "Lars Berscheid") 179 | set(CPACK_PACKAGE_VERSION ${ruckig_VERSION}) 180 | set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE") 181 | set(CPACK_RESOURCE_FILE_README "${CMAKE_CURRENT_SOURCE_DIR}/README.md") 182 | set(CPACK_SYSTEM_NAME ${CMAKE_HOST_SYSTEM_PROCESSOR}) 183 | set(CPACK_DEBIAN_PACKAGE_HOMEPAGE "https://ruckig.com") 184 | set(CPACK_DEBIAN_PACKAGE_MAINTAINER "Lars Berscheid") 185 | set(CPACK_DEBIAN_PACKAGE_VERSION ${CPACK_PACKAGE_VERSION}-1) 186 | set(CPACK_DEBIAN_PACKAGE_CONFLICTS "ros-melodic-ruckig, ros-noetic-ruckig, ros-foxy-ruckig, ros-galactic-ruckig, ros-humble-ruckig, ros-rolling-ruckig") 187 | include(CPack) 188 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Lars Berscheid 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /cmake/ruckig-config.cmake.in: -------------------------------------------------------------------------------- 1 | # Generated by CMake @CMAKE_VERSION@ for @PROJECT_NAME@ 2 | 3 | @PACKAGE_INIT@ 4 | 5 | include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@-targets.cmake") 6 | -------------------------------------------------------------------------------- /doc/benchmark.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pantor/ruckig/a192afc8eb0834c28a2d9464933cb5acf5fc7669/doc/benchmark.png -------------------------------------------------------------------------------- /doc/example_profile.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pantor/ruckig/a192afc8eb0834c28a2d9464933cb5acf5fc7669/doc/example_profile.png -------------------------------------------------------------------------------- /doc/ruckig_toppra_example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pantor/ruckig/a192afc8eb0834c28a2d9464933cb5acf5fc7669/doc/ruckig_toppra_example.png -------------------------------------------------------------------------------- /examples/01_position.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | 6 | using namespace ruckig; 7 | 8 | int main() { 9 | // Create instances: the Ruckig OTG as well as input and output parameters 10 | Ruckig<3> otg(0.01); // control cycle 11 | InputParameter<3> input; 12 | OutputParameter<3> output; 13 | 14 | // Set input parameters 15 | input.current_position = {0.0, 0.0, 0.5}; 16 | input.current_velocity = {0.0, -2.2, -0.5}; 17 | input.current_acceleration = {0.0, 2.5, -0.5}; 18 | 19 | input.target_position = {5.0, -2.0, -3.5}; 20 | input.target_velocity = {0.0, -0.5, -2.0}; 21 | input.target_acceleration = {0.0, 0.0, 0.5}; 22 | 23 | input.max_velocity = {3.0, 1.0, 3.0}; 24 | input.max_acceleration = {3.0, 2.0, 1.0}; 25 | input.max_jerk = {4.0, 3.0, 2.0}; 26 | 27 | // Generate the trajectory within the control loop 28 | std::cout << "t | position" << std::endl; 29 | while (otg.update(input, output) == Result::Working) { 30 | std::cout << output.time << " | " << join(output.new_position) << std::endl; 31 | 32 | output.pass_to_input(input); 33 | } 34 | 35 | std::cout << "Trajectory duration: " << output.trajectory.get_duration() << " [s]." << std::endl; 36 | } 37 | -------------------------------------------------------------------------------- /examples/01_position.py: -------------------------------------------------------------------------------- 1 | from copy import copy 2 | 3 | from ruckig import InputParameter, OutputParameter, Result, Ruckig 4 | 5 | 6 | if __name__ == '__main__': 7 | # Create instances: the Ruckig OTG as well as input and output parameters 8 | otg = Ruckig(3, 0.01) # DoFs, control cycle 9 | inp = InputParameter(3) 10 | out = OutputParameter(3) 11 | 12 | # Set input parameters 13 | inp.current_position = [0.0, 0.0, 0.5] 14 | inp.current_velocity = [0.0, -2.2, -0.5] 15 | inp.current_acceleration = [0.0, 2.5, -0.5] 16 | 17 | inp.target_position = [5.0, -2.0, -3.5] 18 | inp.target_velocity = [0.0, -0.5, -2.0] 19 | inp.target_acceleration = [0.0, 0.0, 0.5] 20 | 21 | inp.max_velocity = [3.0, 1.0, 3.0] 22 | inp.max_acceleration = [3.0, 2.0, 1.0] 23 | inp.max_jerk = [4.0, 3.0, 2.0] 24 | 25 | print('\t'.join(['t'] + [str(i) for i in range(otg.degrees_of_freedom)])) 26 | 27 | # Generate the trajectory within the control loop 28 | first_output, out_list = None, [] 29 | res = Result.Working 30 | while res == Result.Working: 31 | res = otg.update(inp, out) 32 | 33 | print('\t'.join([f'{out.time:0.3f}'] + [f'{p:0.3f}' for p in out.new_position])) 34 | out_list.append(copy(out)) 35 | 36 | out.pass_to_input(inp) 37 | 38 | if not first_output: 39 | first_output = copy(out) 40 | 41 | print(f'Calculation duration: {first_output.calculation_duration:0.1f} [µs]') 42 | print(f'Trajectory duration: {first_output.trajectory.duration:0.4f} [s]') 43 | 44 | # Plot the trajectory 45 | # from pathlib import Path 46 | # from plotter import Plotter 47 | 48 | # project_path = Path(__file__).parent.parent.absolute() 49 | # Plotter.plot_trajectory(project_path / 'examples' / '01_trajectory.pdf', otg, inp, out_list, plot_jerk=False) 50 | -------------------------------------------------------------------------------- /examples/01_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pantor/ruckig/a192afc8eb0834c28a2d9464933cb5acf5fc7669/examples/01_trajectory.pdf -------------------------------------------------------------------------------- /examples/02_position_offline.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | 6 | using namespace ruckig; 7 | 8 | int main() { 9 | // Create input parameters 10 | InputParameter<3> input; 11 | input.current_position = {0.0, 0.0, 0.5}; 12 | input.current_velocity = {0.0, -2.2, -0.5}; 13 | input.current_acceleration = {0.0, 2.5, -0.5}; 14 | 15 | input.target_position = {5.0, -2.0, -3.5}; 16 | input.target_velocity = {0.0, -0.5, -2.0}; 17 | input.target_acceleration = {0.0, 0.0, 0.5}; 18 | 19 | input.max_velocity = {3.0, 1.0, 3.0}; 20 | input.max_acceleration = {3.0, 2.0, 1.0}; 21 | input.max_jerk = {4.0, 3.0, 2.0}; 22 | 23 | // Set different constraints for negative direction 24 | input.min_velocity = {-2.0, -0.5, -3.0}; 25 | input.min_acceleration = {-2.0, -2.0, -2.0}; 26 | 27 | // We don't need to pass the control rate (cycle time) when using only offline features 28 | Ruckig<3> otg; 29 | Trajectory<3> trajectory; 30 | 31 | // Calculate the trajectory in an offline manner (outside of the control loop) 32 | Result result = otg.calculate(input, trajectory); 33 | if (result == Result::ErrorInvalidInput) { 34 | std::cout << "Invalid input!" << std::endl; 35 | return -1; 36 | } 37 | 38 | // Get duration of the trajectory 39 | std::cout << "Trajectory duration: " << trajectory.get_duration() << " [s]." << std::endl; 40 | 41 | double new_time = 1.0; 42 | 43 | // Then, we can calculate the kinematic state at a given time 44 | std::array new_position, new_velocity, new_acceleration; 45 | trajectory.at_time(new_time, new_position, new_velocity, new_acceleration); 46 | 47 | std::cout << "Position at time " << new_time << " [s]: " << join(new_position) << std::endl; 48 | 49 | // Get some info about the position extrema of the trajectory 50 | std::array position_extrema = trajectory.get_position_extrema(); 51 | std::cout << "Position extremas for DoF 4 are " << position_extrema[2].min << " (min) to " << position_extrema[2].max << " (max)" << std::endl; 52 | } 53 | -------------------------------------------------------------------------------- /examples/02_position_offline.py: -------------------------------------------------------------------------------- 1 | from ruckig import InputParameter, Ruckig, Trajectory, Result 2 | 3 | 4 | if __name__ == '__main__': 5 | inp = InputParameter(3) 6 | 7 | inp.current_position = [0.0, 0.0, 0.5] 8 | inp.current_velocity = [0.0, -2.2, -0.5] 9 | inp.current_acceleration = [0.0, 2.5, -0.5] 10 | 11 | inp.target_position = [5.0, -2.0, -3.5] 12 | inp.target_velocity = [0.0, -0.5, -2.0] 13 | inp.target_acceleration = [0.0, 0.0, 0.5] 14 | 15 | inp.max_velocity = [3.0, 1.0, 3.0] 16 | inp.max_acceleration = [3.0, 2.0, 1.0] 17 | inp.max_jerk = [4.0, 3.0, 2.0] 18 | 19 | # Set different constraints for negative direction 20 | inp.min_velocity = [-1.0, -0.5, -3.0] 21 | inp.min_acceleration = [-2.0, -1.0, -2.0] 22 | 23 | # We don't need to pass the control rate (cycle time) when using only offline features 24 | otg = Ruckig(3) 25 | trajectory = Trajectory(3) 26 | 27 | # Calculate the trajectory in an offline manner 28 | result = otg.calculate(inp, trajectory) 29 | if result == Result.ErrorInvalidInput: 30 | raise Exception('Invalid input!') 31 | 32 | print(f'Trajectory duration: {trajectory.duration:0.4f} [s]') 33 | 34 | new_time = 1.0 35 | 36 | # Then, we can calculate the kinematic state at a given time 37 | new_position, new_velocity, new_acceleration = trajectory.at_time(new_time) 38 | 39 | print(f'Position at time {new_time:0.4f} [s]: {new_position}') 40 | 41 | # Get some info about the position extrema of the trajectory 42 | print(f'Position extremas are {trajectory.position_extrema}') 43 | -------------------------------------------------------------------------------- /examples/02_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pantor/ruckig/a192afc8eb0834c28a2d9464933cb5acf5fc7669/examples/02_trajectory.pdf -------------------------------------------------------------------------------- /examples/03_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pantor/ruckig/a192afc8eb0834c28a2d9464933cb5acf5fc7669/examples/03_trajectory.pdf -------------------------------------------------------------------------------- /examples/03_waypoints.cpp: -------------------------------------------------------------------------------- 1 | // This example shows the usage of intermediate waypoints. It will only work with Ruckig Pro or enabled cloud API. 2 | 3 | #include 4 | 5 | #include 6 | 7 | 8 | using namespace ruckig; 9 | 10 | int main() { 11 | const double control_cycle = 0.01; 12 | const size_t DOFs = 3; 13 | const size_t max_number_of_waypoints = 10; // for memory allocation 14 | 15 | // Create instances: the Ruckig OTG as well as input and output parameters 16 | Ruckig otg(control_cycle, max_number_of_waypoints); 17 | InputParameter input; 18 | OutputParameter output(max_number_of_waypoints); 19 | 20 | // Set input parameters 21 | input.current_position = {0.2, 0.0, -0.3}; 22 | input.current_velocity = {0.0, 0.2, 0.0}; 23 | input.current_acceleration = {0.0, 0.6, 0.0}; 24 | 25 | input.intermediate_positions = { 26 | {1.4, -1.6, 1.0}, 27 | {-0.6, -0.5, 0.4}, 28 | {-0.4, -0.35, 0.0}, 29 | {0.8, 1.8, -0.1} 30 | }; 31 | 32 | input.target_position = {0.5, 1.0, 0.0}; 33 | input.target_velocity = {0.2, 0.0, 0.3}; 34 | input.target_acceleration = {0.0, 0.1, -0.1}; 35 | 36 | input.max_velocity = {1.0, 2.0, 1.0}; 37 | input.max_acceleration = {3.0, 2.0, 2.0}; 38 | input.max_jerk = {6.0, 10.0, 20.0}; 39 | 40 | std::cout << "t | position" << std::endl; 41 | double calculation_duration = 0.0; 42 | while (otg.update(input, output) == Result::Working) { 43 | std::cout << output.time << " | " << join(output.new_position) << std::endl; 44 | 45 | output.pass_to_input(input); 46 | 47 | if (output.new_calculation) { 48 | calculation_duration = output.calculation_duration; 49 | } 50 | } 51 | 52 | std::cout << "Reached target position in " << output.trajectory.get_duration() << " [s]." << std::endl; 53 | std::cout << "Calculation in " << calculation_duration << " [µs]." << std::endl; 54 | } 55 | -------------------------------------------------------------------------------- /examples/03_waypoints.py: -------------------------------------------------------------------------------- 1 | # This example shows the usage of intermediate waypoints. It will only work with Ruckig Pro or enabled cloud API (e.g. default when installed by pip / PyPI). 2 | 3 | from copy import copy 4 | 5 | from ruckig import InputParameter, OutputParameter, Result, Ruckig 6 | 7 | 8 | if __name__ == '__main__': 9 | # Create instances: the Ruckig OTG as well as input and output parameters 10 | otg = Ruckig(3, 0.01, 10) # DoFs, control cycle rate, maximum number of intermediate waypoints for memory allocation 11 | inp = InputParameter(3) # DoFs 12 | out = OutputParameter(3, 10) # DoFs, maximum number of intermediate waypoints for memory allocation 13 | 14 | inp.current_position = [0.2, 0, -0.3] 15 | inp.current_velocity = [0, 0.2, 0] 16 | inp.current_acceleration = [0, 0.6, 0] 17 | 18 | inp.intermediate_positions = [ 19 | [1.4, -1.6, 1.0], 20 | [-0.6, -0.5, 0.4], 21 | [-0.4, -0.35, 0.0], 22 | [0.8, 1.8, -0.1], 23 | ] 24 | 25 | inp.target_position = [0.5, 1, 0] 26 | inp.target_velocity = [0.2, 0, 0.3] 27 | inp.target_acceleration = [0, 0.1, -0.1] 28 | 29 | inp.max_velocity = [1, 2, 1] 30 | inp.max_acceleration = [3, 2, 2] 31 | inp.max_jerk = [6, 10, 20] 32 | 33 | print('\t'.join(['t'] + [str(i) for i in range(otg.degrees_of_freedom)])) 34 | 35 | # Generate the trajectory within the control loop 36 | first_output, out_list = None, [] 37 | res = Result.Working 38 | while res == Result.Working: 39 | res = otg.update(inp, out) 40 | 41 | print('\t'.join([f'{out.time:0.3f}'] + [f'{p:0.3f}' for p in out.new_position])) 42 | out_list.append(copy(out)) 43 | 44 | out.pass_to_input(inp) 45 | 46 | if not first_output: 47 | first_output = copy(out) 48 | 49 | print(f'Calculation duration: {first_output.calculation_duration:0.1f} [µs]') 50 | print(f'Trajectory duration: {first_output.trajectory.duration:0.4f} [s]') 51 | 52 | # Plot the trajectory 53 | # from pathlib import Path 54 | # from plotter import Plotter 55 | 56 | # project_path = Path(__file__).parent.parent.absolute() 57 | # Plotter.plot_trajectory(project_path / 'examples' / '03_trajectory.pdf', otg, inp, out_list, plot_jerk=False) 58 | -------------------------------------------------------------------------------- /examples/04_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pantor/ruckig/a192afc8eb0834c28a2d9464933cb5acf5fc7669/examples/04_trajectory.pdf -------------------------------------------------------------------------------- /examples/04_waypoints_online.cpp: -------------------------------------------------------------------------------- 1 | // This example shows the usage of intermediate waypoints. It will only work with Ruckig Pro or enabled cloud API. 2 | 3 | #include 4 | 5 | #include 6 | 7 | 8 | using namespace ruckig; 9 | 10 | int main() { 11 | const double control_cycle = 0.01; 12 | const size_t DOFs = 3; 13 | const size_t max_number_of_waypoints = 10; // for memory allocation 14 | 15 | // Create instances: the Ruckig OTG as well as input and output parameters 16 | Ruckig otg(control_cycle, max_number_of_waypoints); 17 | InputParameter input; 18 | OutputParameter output(max_number_of_waypoints); 19 | 20 | // Set input parameters 21 | input.current_position = {0.2, 0.0, -0.3}; 22 | input.current_velocity = {0.0, 0.2, 0.0}; 23 | input.current_acceleration = {0.0, 0.6, 0.0}; 24 | 25 | input.intermediate_positions = { 26 | {1.4, -1.6, 1.0}, 27 | {-0.6, -0.5, 0.4}, 28 | {-0.4, -0.35, 0.0}, 29 | {0.8, 1.8, -0.1} 30 | }; 31 | 32 | input.target_position = {0.5, 1.0, 0.0}; 33 | input.target_velocity = {0.2, 0.0, 0.3}; 34 | input.target_acceleration = {0.0, 0.1, -0.1}; 35 | 36 | input.max_velocity = {1.0, 2.0, 1.0}; 37 | input.max_acceleration = {3.0, 2.0, 2.0}; 38 | input.max_jerk = {6.0, 10.0, 20.0}; 39 | 40 | input.interrupt_calculation_duration = 500; // [µs] 41 | 42 | std::cout << "t | position" << std::endl; 43 | double calculation_duration = 0.0; 44 | while (otg.update(input, output) == Result::Working) { 45 | if (output.new_calculation) { 46 | std::cout << "Updated the trajectory:" << std::endl; 47 | std::cout << " Reached target position in " << output.trajectory.get_duration() << " [s]." << std::endl; 48 | std::cout << " Calculation in " << output.calculation_duration << " [µs]." << std::endl; 49 | } 50 | 51 | std::cout << output.time << " | " << join(output.new_position) << std::endl; 52 | 53 | output.pass_to_input(input); 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /examples/04_waypoints_online.py: -------------------------------------------------------------------------------- 1 | # This example shows the usage of intermediate waypoints. It will only work with Ruckig Pro or enabled cloud API (e.g. default when installed by pip / PyPI). 2 | 3 | from copy import copy 4 | 5 | from ruckig import InputParameter, OutputParameter, Result, Ruckig 6 | 7 | 8 | if __name__ == '__main__': 9 | # Create instances: the Ruckig OTG as well as input and output parameters 10 | otg = Ruckig(3, 0.01, 10) # DoFs, control cycle rate, maximum number of intermediate waypoints for memory allocation 11 | inp = InputParameter(3) # DoFs 12 | out = OutputParameter(3, 10) # DoFs, maximum number of intermediate waypoints for memory allocation 13 | 14 | inp.current_position = [0.2, 0, -0.3] 15 | inp.current_velocity = [0, 0.2, 0] 16 | inp.current_acceleration = [0, 0.6, 0] 17 | 18 | inp.intermediate_positions = [ 19 | [1.4, -1.6, 1.0], 20 | [-0.6, -0.5, 0.4], 21 | [-0.4, -0.35, 0.0], 22 | [0.8, 1.8, -0.1], 23 | ] 24 | 25 | inp.target_position = [0.5, 1, 0] 26 | inp.target_velocity = [0.2, 0, 0.3] 27 | inp.target_acceleration = [0, 0.1, -0.1] 28 | 29 | inp.max_velocity = [1, 2, 1] 30 | inp.max_acceleration = [3, 2, 2] 31 | inp.max_jerk = [6, 10, 20] 32 | 33 | inp.interrupt_calculation_duration = 500 # [µs] 34 | 35 | print('\t'.join(['t'] + [str(i) for i in range(otg.degrees_of_freedom)])) 36 | 37 | # Generate the trajectory within the control loop 38 | out_list = [] 39 | res = Result.Working 40 | while res == Result.Working: 41 | res = otg.update(inp, out) 42 | 43 | if out.new_calculation: 44 | print('Updated the trajectory:') 45 | print(f' Calculation duration: {out.calculation_duration:0.1f} [µs]') 46 | print(f' Trajectory duration: {out.trajectory.duration:0.4f} [s]') 47 | 48 | print('\t'.join([f'{out.time:0.3f}'] + [f'{p:0.3f}' for p in out.new_position])) 49 | out_list.append(copy(out)) 50 | 51 | out.pass_to_input(inp) 52 | 53 | # Plot the trajectory 54 | # from pathlib import Path 55 | # from plotter import Plotter 56 | 57 | # project_path = Path(__file__).parent.parent.absolute() 58 | # Plotter.plot_trajectory(project_path / 'examples' / '04_trajectory.pdf', otg, inp, out_list, plot_jerk=False) 59 | -------------------------------------------------------------------------------- /examples/05_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pantor/ruckig/a192afc8eb0834c28a2d9464933cb5acf5fc7669/examples/05_trajectory.pdf -------------------------------------------------------------------------------- /examples/05_velocity.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | 6 | using namespace ruckig; 7 | 8 | int main() { 9 | // Create instances: the Ruckig OTG as well as input and output parameters 10 | Ruckig<3> otg(0.01); // control cycle 11 | InputParameter<3> input; 12 | OutputParameter<3> output; 13 | 14 | // Set input parameters and velocity control interface 15 | input.control_interface = ControlInterface::Velocity; 16 | 17 | input.current_position = {0.0, 0.0, 0.5}; 18 | input.current_velocity = {3.0, -2.2, -0.5}; 19 | input.current_acceleration = {0.0, 2.5, -0.5}; 20 | 21 | input.target_velocity = {0.0, -0.5, -1.5}; 22 | input.target_acceleration = {0.0, 0.0, 0.5}; 23 | 24 | input.max_acceleration = {3.0, 2.0, 1.0}; 25 | input.max_jerk = {6.0, 6.0, 4.0}; 26 | 27 | // Generate the trajectory within the control loop 28 | std::cout << "t | position" << std::endl; 29 | while (otg.update(input, output) == Result::Working) { 30 | std::cout << output.time << " | " << join(output.new_position) << std::endl; 31 | 32 | output.pass_to_input(input); 33 | } 34 | 35 | std::cout << "Trajectory duration: " << output.trajectory.get_duration() << " [s]." << std::endl; 36 | } 37 | -------------------------------------------------------------------------------- /examples/05_velocity.py: -------------------------------------------------------------------------------- 1 | from copy import copy 2 | 3 | from ruckig import InputParameter, OutputParameter, Result, Ruckig, ControlInterface 4 | 5 | 6 | if __name__ == '__main__': 7 | # Create instances: the Ruckig OTG as well as input and output parameters 8 | otg = Ruckig(3, 0.01) # DoFs, control cycle 9 | inp = InputParameter(3) 10 | out = OutputParameter(3) 11 | 12 | inp.control_interface = ControlInterface.Velocity 13 | 14 | inp.current_position = [0.0, 0.0, 0.5] 15 | inp.current_velocity = [3.0, -2.2, -0.5] 16 | inp.current_acceleration = [0.0, 2.5, -0.5] 17 | 18 | inp.target_velocity = [0.0, -0.5, -1.5] 19 | inp.target_acceleration = [0.0, 0.0, 0.5] 20 | 21 | inp.max_acceleration = [3.0, 2.0, 1.0] 22 | inp.max_jerk = [6.0, 6.0, 4.0] 23 | 24 | print('\t'.join(['t'] + [str(i) for i in range(otg.degrees_of_freedom)])) 25 | 26 | # Generate the trajectory within the control loop 27 | first_output, out_list = None, [] 28 | res = Result.Working 29 | while res == Result.Working: 30 | res = otg.update(inp, out) 31 | 32 | print('\t'.join([f'{out.time:0.3f}'] + [f'{p:0.3f}' for p in out.new_position])) 33 | out_list.append(copy(out)) 34 | 35 | out.pass_to_input(inp) 36 | 37 | if not first_output: 38 | first_output = copy(out) 39 | 40 | print(f'Calculation duration: {first_output.calculation_duration:0.1f} [µs]') 41 | print(f'Trajectory duration: {first_output.trajectory.duration:0.4f} [s]') 42 | 43 | # Plot the trajectory 44 | # from pathlib import Path 45 | # from plotter import Plotter 46 | 47 | # project_path = Path(__file__).parent.parent.absolute() 48 | # Plotter.plot_trajectory(project_path / 'examples' / '05_trajectory.pdf', otg, inp, out_list, plot_jerk=False) 49 | -------------------------------------------------------------------------------- /examples/06_stop.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | 6 | using namespace ruckig; 7 | 8 | int main() { 9 | // Create instances: the ruckig otg as well as input and output parameters 10 | Ruckig<3> otg(0.01); 11 | InputParameter<3> input; 12 | OutputParameter<3> output; 13 | 14 | // Set input parameters 15 | input.current_position = {0.0, 0.0, 0.5}; 16 | input.current_velocity = {0.0, -2.2, -0.5}; 17 | input.current_acceleration = {0.0, 2.5, -0.5}; 18 | 19 | input.target_position = {5.0, -2.0, -3.5}; 20 | input.target_velocity = {0.0, -0.5, -2.0}; 21 | input.target_acceleration = {0.0, 0.0, 0.5}; 22 | 23 | input.max_velocity = {3.0, 1.0, 3.0}; 24 | input.max_acceleration = {3.0, 2.0, 1.0}; 25 | input.max_jerk = {4.0, 3.0, 2.0}; 26 | 27 | // Generate the trajectory within the control loop 28 | std::cout << "t | position" << std::endl; 29 | bool on_stop_trajectory = false; 30 | while (otg.update(input, output) == Result::Working) { 31 | std::cout << output.time << " | " << join(output.new_position) << std::endl; 32 | 33 | // Activate stop trajectory after 1s 34 | if (output.time >= 1.0 && !on_stop_trajectory) { 35 | std::cout << "Stop immediately." << std::endl; 36 | on_stop_trajectory = true; 37 | 38 | // Synchronization is disabled so that each DoF stops as fast as possible independently 39 | input.control_interface = ControlInterface::Velocity; 40 | input.synchronization = Synchronization::None; 41 | input.target_velocity = {0.0, 0.0, 0.0}; 42 | input.target_acceleration = {0.0, 0.0, 0.0}; 43 | input.max_jerk = {12.0, 10.0, 8.0}; 44 | } 45 | 46 | output.pass_to_input(input); 47 | } 48 | 49 | std::cout << "Stop trajectory duration: " << output.trajectory.get_duration() << " [s]." << std::endl; 50 | } 51 | -------------------------------------------------------------------------------- /examples/06_stop.py: -------------------------------------------------------------------------------- 1 | from copy import copy 2 | 3 | from ruckig import InputParameter, OutputParameter, Result, Ruckig, ControlInterface, Synchronization 4 | 5 | 6 | if __name__ == '__main__': 7 | # Create instances: the Ruckig OTG as well as input and output parameters 8 | otg = Ruckig(3, 0.01) # DoFs, control cycle 9 | inp = InputParameter(3) 10 | out = OutputParameter(3) 11 | 12 | inp.current_position = [0.0, 0.0, 0.5] 13 | inp.current_velocity = [0.0, -2.2, -0.5] 14 | inp.current_acceleration = [0.0, 2.5, -0.5] 15 | 16 | inp.target_position = [5.0, -2.0, -3.5] 17 | inp.target_velocity = [0.0, -0.5, -2.0] 18 | inp.target_acceleration = [0.0, 0.0, 0.5] 19 | 20 | inp.max_velocity = [3.0, 1.0, 3.0] 21 | inp.max_acceleration = [3.0, 2.0, 1.0] 22 | inp.max_jerk = [4.0, 3.0, 2.0] 23 | 24 | print('\t'.join(['t'] + [str(i) for i in range(otg.degrees_of_freedom)])) 25 | 26 | # Generate the trajectory within the control loop 27 | first_output, out_list, time_offsets = None, [], [] 28 | on_stop_trajectory = False 29 | res = Result.Working 30 | while res == Result.Working: 31 | res = otg.update(inp, out) 32 | 33 | print('\t'.join([f'{out.time:0.3f}'] + [f'{p:0.3f}' for p in out.new_position])) 34 | out_list.append(copy(out)) 35 | time_offsets.append(1.0 if on_stop_trajectory else 0.0) 36 | 37 | # Activate stop trajectory after 1s 38 | if out.time >= 1.0 and not on_stop_trajectory: 39 | print('Stop immediately!') 40 | on_stop_trajectory = True 41 | 42 | # Synchronization is disabled so that each DoF stops as fast as possible independently 43 | inp.control_interface = ControlInterface.Velocity 44 | inp.synchronization = Synchronization.No 45 | inp.target_velocity = [0.0, 0.0, 0.0] 46 | inp.target_acceleration = [0.0, 0.0, 0.0] 47 | inp.max_jerk = [12.0, 10.0, 8.0] 48 | 49 | out.pass_to_input(inp) 50 | 51 | if not first_output: 52 | first_output = copy(out) 53 | 54 | print(f'Calculation duration: {first_output.calculation_duration:0.1f} [µs]') 55 | print(f'Trajectory duration: {first_output.trajectory.duration:0.4f} [s]') 56 | 57 | # Plot the trajectory 58 | # from pathlib import Path 59 | # from plotter import Plotter 60 | 61 | # project_path = Path(__file__).parent.parent.absolute() 62 | # Plotter.plot_trajectory(project_path / 'examples' / '06_trajectory.pdf', otg, inp, out_list, plot_jerk=False, time_offsets=time_offsets) 63 | -------------------------------------------------------------------------------- /examples/06_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pantor/ruckig/a192afc8eb0834c28a2d9464933cb5acf5fc7669/examples/06_trajectory.pdf -------------------------------------------------------------------------------- /examples/07_minimum_duration.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | 6 | using namespace ruckig; 7 | 8 | int main() { 9 | // Create instances: the Ruckig OTG as well as input and output parameters 10 | Ruckig<3> otg(0.01); // control cycle 11 | InputParameter<3> input; 12 | OutputParameter<3> output; 13 | 14 | // Set input parameters 15 | input.current_position = {0.0, 0.0, 0.5}; 16 | input.current_velocity = {0.0, -2.2, -0.5}; 17 | input.current_acceleration = {0.0, 2.5, -0.5}; 18 | 19 | input.target_position = {-5.0, -2.0, -3.5}; 20 | input.target_velocity = {0.0, -0.5, -2.0}; 21 | input.target_acceleration = {0.0, 0.0, 0.5}; 22 | 23 | input.max_velocity = {3.0, 1.0, 3.0}; 24 | input.max_acceleration = {3.0, 2.0, 1.0}; 25 | input.max_jerk = {4.0, 3.0, 2.0}; 26 | 27 | // Set minimum duration (equals the trajectory duration when target velocity and acceleration are zero) 28 | input.minimum_duration = 5.0; 29 | 30 | // Generate the trajectory within the control loop 31 | std::cout << "t | position" << std::endl; 32 | while (otg.update(input, output) == Result::Working) { 33 | std::cout << output.time << " | " << join(output.new_position) << std::endl; 34 | 35 | output.pass_to_input(input); 36 | } 37 | 38 | std::cout << "Trajectory duration: " << output.trajectory.get_duration() << " [s]." << std::endl; 39 | } 40 | -------------------------------------------------------------------------------- /examples/07_minimum_duration.py: -------------------------------------------------------------------------------- 1 | from copy import copy 2 | 3 | from ruckig import InputParameter, OutputParameter, Result, Ruckig 4 | 5 | 6 | if __name__ == '__main__': 7 | otg = Ruckig(3, 0.01) 8 | inp = InputParameter(3) 9 | out = OutputParameter(3) 10 | 11 | # Set input parameters 12 | inp.current_position = [0.0, 0.0, 0.5] 13 | inp.current_velocity = [0.0, -2.2, -0.5] 14 | inp.current_acceleration = [0.0, 2.5, -0.5] 15 | 16 | inp.target_position = [-5.0, -2.0, -3.5] 17 | inp.target_velocity = [0.0, -0.5, -2.0] 18 | inp.target_acceleration = [0.0, 0.0, 0.5] 19 | 20 | inp.max_velocity = [3.0, 1.0, 3.0] 21 | inp.max_acceleration = [3.0, 2.0, 1.0] 22 | inp.max_jerk = [4.0, 3.0, 2.0] 23 | 24 | # Set minimum duration (equals the trajectory duration when target velocity and acceleration are zero) 25 | inp.minimum_duration = 5.0 26 | 27 | print('\t'.join(['t'] + [str(i) for i in range(otg.degrees_of_freedom)])) 28 | 29 | # Generate the trajectory within the control loop 30 | first_output, out_list = None, [] 31 | res = Result.Working 32 | while res == Result.Working: 33 | res = otg.update(inp, out) 34 | 35 | print('\t'.join([f'{out.time:0.3f}'] + [f'{p:0.3f}' for p in out.new_position])) 36 | out_list.append(copy(out)) 37 | 38 | out.pass_to_input(inp) 39 | 40 | if not first_output: 41 | first_output = copy(out) 42 | 43 | print(f'Calculation duration: {first_output.calculation_duration:0.1f} [µs]') 44 | print(f'Trajectory duration: {first_output.trajectory.duration:0.4f} [s]') 45 | 46 | # Plot the trajectory 47 | # from pathlib import Path 48 | # from plotter import Plotter 49 | 50 | # project_path = Path(__file__).parent.parent.absolute() 51 | # Plotter.plot_trajectory(project_path / 'examples' / '07_trajectory.pdf', otg, inp, out_list, plot_jerk=False) 52 | -------------------------------------------------------------------------------- /examples/07_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pantor/ruckig/a192afc8eb0834c28a2d9464933cb5acf5fc7669/examples/07_trajectory.pdf -------------------------------------------------------------------------------- /examples/08_per_section_minimum_duration.cpp: -------------------------------------------------------------------------------- 1 | // This example shows the usage of intermediate waypoints. It will only work with Ruckig Pro or enabled cloud API. 2 | 3 | #include 4 | 5 | #include 6 | 7 | 8 | using namespace ruckig; 9 | 10 | int main() { 11 | const double control_cycle = 0.01; 12 | const size_t DOFs = 3; 13 | const size_t max_number_of_waypoints = 10; // for memory allocation 14 | 15 | // Create instances: the Ruckig OTG as well as input and output parameters 16 | Ruckig otg(control_cycle, max_number_of_waypoints); 17 | InputParameter input; 18 | OutputParameter output(max_number_of_waypoints); 19 | 20 | // Set input parameters 21 | input.current_position = {0.8, 0.0, 0.5}; 22 | input.current_velocity = {0.0, 0.0, 0.0}; 23 | input.current_acceleration = {0.0, 0.0, 0.0}; 24 | 25 | input.intermediate_positions = { 26 | {1.4, -1.6, 1.0}, 27 | {-0.6, -0.5, 0.4}, 28 | {-0.4, -0.35, 0.0}, 29 | {-0.2, 0.35, -0.1}, 30 | {0.2, 0.5, -0.1}, 31 | {0.8, 1.8, -0.1} 32 | }; 33 | 34 | input.target_position = {0.5, 1.2, 0.0}; 35 | input.target_velocity = {0.0, 0.0, 0.0}; 36 | input.target_acceleration = {0.0, 0.0, 0.0}; 37 | 38 | input.max_velocity = {3.0, 2.0, 2.0}; 39 | input.max_acceleration = {6.0, 4.0, 4.0}; 40 | input.max_jerk = {16.0, 10.0, 20.0}; 41 | 42 | // Define a minimum duration per section of the trajectory (number waypoints + 1) 43 | input.per_section_minimum_duration = {0, 2.0, 0.0, 1.0, 0.0, 2.0, 0}; 44 | 45 | std::cout << "t | position" << std::endl; 46 | double calculation_duration = 0.0; 47 | while (otg.update(input, output) == Result::Working) { 48 | std::cout << output.time << " | " << join(output.new_position) << std::endl; 49 | 50 | output.pass_to_input(input); 51 | 52 | if (output.new_calculation) { 53 | calculation_duration = output.calculation_duration; 54 | } 55 | } 56 | 57 | std::cout << "Reached target position in " << output.trajectory.get_duration() << " [s]." << std::endl; 58 | std::cout << "Calculation in " << calculation_duration << " [µs]." << std::endl; 59 | } 60 | -------------------------------------------------------------------------------- /examples/08_per_section_minimum_duration.py: -------------------------------------------------------------------------------- 1 | # This example shows the usage of intermediate waypoints. It will only work with Ruckig Pro or enabled cloud API (e.g. default when installed by pip / PyPI). 2 | 3 | from copy import copy 4 | 5 | from ruckig import InputParameter, OutputParameter, Result, Ruckig 6 | 7 | 8 | if __name__ == '__main__': 9 | # Create instances: the Ruckig OTG as well as input and output parameters 10 | otg = Ruckig(3, 0.01, 10) # DoFs, control cycle rate, maximum number of intermediate waypoints for memory allocation 11 | inp = InputParameter(3) # DoFs 12 | out = OutputParameter(3, 10) # DoFs, maximum number of intermediate waypoints for memory allocation 13 | 14 | inp.current_position = [0.8, 0, 0.5] 15 | inp.current_velocity = [0, 0, 0] 16 | inp.current_acceleration = [0, 0, 0] 17 | 18 | inp.intermediate_positions = [ 19 | [1.4, -1.6, 1.0], 20 | [-0.6, -0.5, 0.4], 21 | [-0.4, -0.35, 0.0], 22 | [-0.2, 0.35, -0.1], 23 | [0.2, 0.5, -0.1], 24 | [0.8, 1.8, -0.1], 25 | ] 26 | 27 | inp.target_position = [0.5, 1.2, 0] 28 | inp.target_velocity = [0, 0, 0] 29 | inp.target_acceleration = [0, 0, 0] 30 | 31 | inp.max_velocity = [3, 2, 2] 32 | inp.max_acceleration = [6, 4, 4] 33 | inp.max_jerk = [16, 10, 20] 34 | 35 | # Define a minimum duration per section of the trajectory (number waypoints + 1) 36 | inp.per_section_minimum_duration = [0, 2.0, 0.0, 1.0, 0.0, 2.0, 0] 37 | 38 | print('\t'.join(['t'] + [str(i) for i in range(otg.degrees_of_freedom)])) 39 | 40 | # Generate the trajectory within the control loop 41 | first_output, out_list = None, [] 42 | res = Result.Working 43 | while res == Result.Working: 44 | res = otg.update(inp, out) 45 | 46 | print('\t'.join([f'{out.time:0.3f}'] + [f'{p:0.3f}' for p in out.new_position])) 47 | out_list.append(copy(out)) 48 | 49 | out.pass_to_input(inp) 50 | 51 | if not first_output: 52 | first_output = copy(out) 53 | 54 | print(f'Calculation duration: {first_output.calculation_duration:0.1f} [µs]') 55 | print(f'Trajectory duration: {first_output.trajectory.duration:0.4f} [s]') 56 | 57 | # Plot the trajectory 58 | # from pathlib import Path 59 | # from plotter import Plotter 60 | 61 | # project_path = Path(__file__).parent.parent.absolute() 62 | # Plotter.plot_trajectory(project_path / 'examples' / '08_trajectory.pdf', otg, inp, out_list, plot_jerk=False) 63 | -------------------------------------------------------------------------------- /examples/08_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pantor/ruckig/a192afc8eb0834c28a2d9464933cb5acf5fc7669/examples/08_trajectory.pdf -------------------------------------------------------------------------------- /examples/09_dynamic_dofs.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | 6 | using namespace ruckig; 7 | 8 | int main() { 9 | // Create instances: the Ruckig OTG as well as input and output parameters 10 | size_t degrees_of_freedom = 3; 11 | 12 | Ruckig otg(degrees_of_freedom, 0.01); 13 | InputParameter input(degrees_of_freedom); 14 | OutputParameter output(degrees_of_freedom); 15 | 16 | // Set input parameters 17 | input.current_position = {0.0, 0.0, 0.5}; 18 | input.current_velocity = {0.0, -2.2, -0.5}; 19 | input.current_acceleration = {0.0, 2.5, -0.5}; 20 | 21 | input.target_position = {5.0, -2.0, -3.5}; 22 | input.target_velocity = {0.0, -0.5, -2.0}; 23 | input.target_acceleration = {0.0, 0.0, 0.5}; 24 | 25 | input.max_velocity = {3.0, 1.0, 3.0}; 26 | input.max_acceleration = {3.0, 2.0, 1.0}; 27 | input.max_jerk = {4.0, 3.0, 2.0}; 28 | 29 | // Generate the trajectory within the control loop 30 | std::cout << "t | position" << std::endl; 31 | while (otg.update(input, output) == Result::Working) { 32 | std::cout << output.time << " | " << join(output.new_position) << std::endl; 33 | 34 | output.pass_to_input(input); 35 | } 36 | 37 | std::cout << "Trajectory duration: " << output.trajectory.get_duration() << " [s]" << std::endl; 38 | } 39 | -------------------------------------------------------------------------------- /examples/09_dynamic_dofs.py: -------------------------------------------------------------------------------- 1 | # --- 2 | # 3 | # Nothing to see here, as the Python version *always* uses a dynamic number of degrees of freedom. 4 | # 5 | # --- 6 | -------------------------------------------------------------------------------- /examples/09_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pantor/ruckig/a192afc8eb0834c28a2d9464933cb5acf5fc7669/examples/09_trajectory.pdf -------------------------------------------------------------------------------- /examples/10_dynamic_dofs_waypoints.cpp: -------------------------------------------------------------------------------- 1 | // This example shows the usage of intermediate waypoints. It will only work with Ruckig Pro or enabled cloud API. 2 | 3 | #include 4 | 5 | #include 6 | 7 | 8 | using namespace ruckig; 9 | 10 | int main() { 11 | double control_cycle = 0.01; 12 | size_t DOFs = 3; 13 | size_t max_number_of_waypoints = 10; // for memory allocation 14 | 15 | // Create instances: the Ruckig OTG as well as input and output parameters 16 | Ruckig otg(DOFs, control_cycle, max_number_of_waypoints); 17 | InputParameter input(DOFs); 18 | OutputParameter output(DOFs, max_number_of_waypoints); 19 | 20 | // Set input parameters 21 | input.current_position = {0.2, 0.0, -0.3}; 22 | input.current_velocity = {0.0, 0.2, 0.0}; 23 | input.current_acceleration = {0.0, 0.6, 0.0}; 24 | 25 | input.intermediate_positions = { 26 | {1.4, -1.6, 1.0}, 27 | {-0.6, -0.5, 0.4}, 28 | {-0.4, -0.35, 0.0}, 29 | {0.8, 1.8, -0.1} 30 | }; 31 | 32 | input.target_position = {0.5, 1.0, 0.0}; 33 | input.target_velocity = {0.2, 0.0, 0.3}; 34 | input.target_acceleration = {0.0, 0.1, -0.1}; 35 | 36 | input.max_velocity = {1.0, 2.0, 1.0}; 37 | input.max_acceleration = {3.0, 2.0, 2.0}; 38 | input.max_jerk = {6.0, 10.0, 20.0}; 39 | 40 | std::cout << "t | position" << std::endl; 41 | double calculation_duration = 0.0; 42 | while (otg.update(input, output) == Result::Working) { 43 | std::cout << output.time << " | " << join(output.new_position) << std::endl; 44 | 45 | output.pass_to_input(input); 46 | 47 | if (output.new_calculation) { 48 | calculation_duration = output.calculation_duration; 49 | } 50 | } 51 | 52 | std::cout << "Reached target position in " << output.trajectory.get_duration() << " [s]." << std::endl; 53 | std::cout << "Calculation in " << calculation_duration << " [µs]." << std::endl; 54 | } 55 | -------------------------------------------------------------------------------- /examples/10_dynamic_dofs_waypoints.py: -------------------------------------------------------------------------------- 1 | # --- 2 | # 3 | # Nothing to see here, as the Python version *always* uses a dynamic number of degrees of freedom. 4 | # 5 | # --- 6 | -------------------------------------------------------------------------------- /examples/10_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pantor/ruckig/a192afc8eb0834c28a2d9464933cb5acf5fc7669/examples/10_trajectory.pdf -------------------------------------------------------------------------------- /examples/11_eigen_vector_type.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // Include Eigen before Ruckig 4 | #include // Version 3.4 or later 5 | 6 | #include 7 | 8 | 9 | using namespace ruckig; 10 | 11 | int main() { 12 | // Create instances: the Ruckig OTG as well as input and output parameters 13 | Ruckig<3, EigenVector> otg(0.01); // control cycle 14 | InputParameter<3, EigenVector> input; 15 | OutputParameter<3, EigenVector> output; 16 | 17 | Eigen::Vector3d start_position; 18 | start_position << 0.0, 0.0, 0.5; // Eigen 3.4 also supports construction via an initializer list... 19 | 20 | Eigen::Vector3d position_diff; 21 | position_diff << 5.0, -2.0, -4.0; 22 | 23 | // Set input parameters 24 | input.current_position = start_position; 25 | input.current_velocity = {0.0, -2.2, -0.5}; 26 | input.current_acceleration = {0.0, 2.5, -0.5}; 27 | 28 | input.target_position = start_position + position_diff; 29 | input.target_velocity = {0.0, -0.5, -2.0}; 30 | input.target_acceleration = {0.0, 0.0, 0.5}; 31 | 32 | input.max_velocity = {3.0, 1.0, 3.0}; 33 | input.max_acceleration = {3.0, 2.0, 1.0}; 34 | input.max_jerk = {4.0, 3.0, 2.0}; 35 | 36 | // Generate the trajectory within the control loop 37 | std::cout << "t | position" << std::endl; 38 | while (otg.update(input, output) == Result::Working) { 39 | std::cout << output.time << " | " << join(output.new_position) << std::endl; 40 | 41 | output.pass_to_input(input); 42 | } 43 | 44 | std::cout << "Trajectory duration: " << output.trajectory.get_duration() << " [s]." << std::endl; 45 | } 46 | -------------------------------------------------------------------------------- /examples/11_eigen_vector_type.py: -------------------------------------------------------------------------------- 1 | # --- 2 | # 3 | # Nothing to see here, as the custom vector types don't affect the Python version. 4 | # 5 | # --- 6 | -------------------------------------------------------------------------------- /examples/11_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pantor/ruckig/a192afc8eb0834c28a2d9464933cb5acf5fc7669/examples/11_trajectory.pdf -------------------------------------------------------------------------------- /examples/12_custom_vector_type.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | 6 | template 7 | class MinimalVector { 8 | T data[DOFs]; 9 | 10 | public: 11 | MinimalVector() { } 12 | MinimalVector(std::initializer_list a) { 13 | std::copy_n(a.begin(), DOFs, std::begin(data)); 14 | } 15 | 16 | T operator[](size_t i) const { 17 | return data[i]; 18 | } 19 | 20 | T& operator[](size_t i) { 21 | return data[i]; 22 | } 23 | 24 | size_t size() const { 25 | return DOFs; 26 | } 27 | 28 | bool operator==(const MinimalVector& rhs) const { 29 | for (size_t dof = 0; dof < DOFs; ++dof) { 30 | if (data[dof] != rhs[dof]) { 31 | return false; 32 | } 33 | } 34 | return true; 35 | } 36 | }; 37 | 38 | 39 | using namespace ruckig; 40 | 41 | int main() { 42 | // Create instances: the Ruckig OTG as well as input and output parameters 43 | Ruckig<3, MinimalVector> otg(0.01); // control cycle 44 | InputParameter<3, MinimalVector> input; 45 | OutputParameter<3, MinimalVector> output; 46 | 47 | // Set input parameters 48 | input.current_position = {0.0, 0.0, 0.5}; 49 | input.current_velocity = {0.0, -2.2, -0.5}; 50 | input.current_acceleration = {0.0, 2.5, -0.5}; 51 | 52 | input.target_position = {5.0, -2.0, -3.5}; 53 | input.target_velocity = {0.0, -0.5, -2.0}; 54 | input.target_acceleration = {0.0, 0.0, 0.5}; 55 | 56 | input.max_velocity = {3.0, 1.0, 3.0}; 57 | input.max_acceleration = {3.0, 2.0, 1.0}; 58 | input.max_jerk = {4.0, 3.0, 2.0}; 59 | 60 | // Generate the trajectory within the control loop 61 | std::cout << "t | position" << std::endl; 62 | while (otg.update(input, output) == Result::Working) { 63 | std::cout << output.time << " | " << join(output.new_position) << std::endl; 64 | 65 | output.pass_to_input(input); 66 | } 67 | 68 | std::cout << "Trajectory duration: " << output.trajectory.get_duration() << " [s]." << std::endl; 69 | } 70 | -------------------------------------------------------------------------------- /examples/12_custom_vector_type.py: -------------------------------------------------------------------------------- 1 | # --- 2 | # 3 | # Nothing to see here, as the custom vector types don't affect the Python version. 4 | # 5 | # --- 6 | -------------------------------------------------------------------------------- /examples/12_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pantor/ruckig/a192afc8eb0834c28a2d9464933cb5acf5fc7669/examples/12_trajectory.pdf -------------------------------------------------------------------------------- /examples/13_custom_vector_type_dynamic_dofs.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | 7 | template 8 | class MinimalDynamicDofsVector { 9 | std::deque data; 10 | 11 | public: 12 | MinimalDynamicDofsVector() { } 13 | MinimalDynamicDofsVector(std::initializer_list a) { 14 | data.resize(a.size()); 15 | std::copy_n(a.begin(), a.size(), std::begin(data)); 16 | } 17 | 18 | T operator[](size_t i) const { 19 | return data[i]; 20 | } 21 | 22 | T& operator[](size_t i) { 23 | return data[i]; 24 | } 25 | 26 | size_t size() const { 27 | return data.size(); 28 | } 29 | 30 | void resize(size_t size) { 31 | data.resize(size); 32 | } 33 | 34 | bool operator==(const MinimalDynamicDofsVector& rhs) const { 35 | for (size_t dof = 0; dof < data.size(); ++dof) { 36 | if (data[dof] != rhs[dof]) { 37 | return false; 38 | } 39 | } 40 | return true; 41 | } 42 | }; 43 | 44 | 45 | using namespace ruckig; 46 | 47 | int main() { 48 | // Create instances: the Ruckig OTG as well as input and output parameters 49 | Ruckig otg(3, 0.01); // control cycle 50 | InputParameter input(3); 51 | OutputParameter output(3); 52 | 53 | // Set input parameters 54 | input.current_position = {0.0, 0.0, 0.5}; 55 | input.current_velocity = {0.0, -2.2, -0.5}; 56 | input.current_acceleration = {0.0, 2.5, -0.5}; 57 | 58 | input.target_position = {5.0, -2.0, -3.5}; 59 | input.target_velocity = {0.0, -0.5, -2.0}; 60 | input.target_acceleration = {0.0, 0.0, 0.5}; 61 | 62 | input.max_velocity = {3.0, 1.0, 3.0}; 63 | input.max_acceleration = {3.0, 2.0, 1.0}; 64 | input.max_jerk = {4.0, 3.0, 2.0}; 65 | 66 | // Generate the trajectory within the control loop 67 | std::cout << "t | position" << std::endl; 68 | while (otg.update(input, output) == Result::Working) { 69 | std::cout << output.time << " | " << join(output.new_position) << std::endl; 70 | 71 | output.pass_to_input(input); 72 | } 73 | 74 | std::cout << "Trajectory duration: " << output.trajectory.get_duration() << " [s]." << std::endl; 75 | } 76 | -------------------------------------------------------------------------------- /examples/13_custom_vector_type_dynamic_dofs.py: -------------------------------------------------------------------------------- 1 | # --- 2 | # 3 | # Nothing to see here, as the custom vector types don't affect the Python version. 4 | # 5 | # --- 6 | -------------------------------------------------------------------------------- /examples/13_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pantor/ruckig/a192afc8eb0834c28a2d9464933cb5acf5fc7669/examples/13_trajectory.pdf -------------------------------------------------------------------------------- /examples/14_tracking.cpp: -------------------------------------------------------------------------------- 1 | // Only with Ruckig Pro 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | 9 | using namespace ruckig; 10 | 11 | // Create the target state signal 12 | TargetState<1> model_ramp(double t, double ramp_vel=0.5, double ramp_pos=1.0) { 13 | TargetState<1> target; 14 | const bool on_ramp = t < ramp_pos / std::abs(ramp_vel); 15 | target.position[0] = on_ramp ? t * ramp_vel : ramp_pos; 16 | target.velocity[0] = on_ramp ? ramp_vel : 0.0; 17 | target.acceleration[0] = 0.0; 18 | return target; 19 | } 20 | 21 | TargetState<1> model_constant_acceleration(double t, double ramp_acc=0.05) { 22 | TargetState<1> target; 23 | target.position[0] = t * t * ramp_acc; 24 | target.velocity[0] = t * ramp_acc; 25 | target.acceleration[0] = ramp_acc; 26 | return target; 27 | } 28 | 29 | TargetState<1> model_sinus(double t, double ramp_vel=0.4) { 30 | TargetState<1> target; 31 | target.position[0] = std::sin(ramp_vel * t); 32 | target.velocity[0] = ramp_vel * std::cos(ramp_vel * t); 33 | target.acceleration[0] = -ramp_vel * ramp_vel * std::sin(ramp_vel * t); 34 | return target; 35 | } 36 | 37 | 38 | int main() { 39 | // Create instances: the Trackig OTG as well as input and output parameters 40 | Trackig<1> otg(0.01); // control cycle 41 | InputParameter<1> input; 42 | OutputParameter<1> output; 43 | 44 | // Set input parameters 45 | input.current_position = {0.0}; 46 | input.current_velocity = {0.0}; 47 | input.current_acceleration = {0.0}; 48 | 49 | input.max_velocity = {0.8}; 50 | input.max_acceleration = {2.0}; 51 | input.max_jerk = {5.0}; 52 | 53 | // Optional minimum and maximum position 54 | input.min_position = {-2.5}; 55 | input.max_position = {2.5}; 56 | 57 | otg.reactiveness = 1.0; // default value, should be in [0, 1] 58 | 59 | // Generate the trajectory following the target state 60 | std::cout << "target | follow" << std::endl; 61 | for (size_t t = 0; t < 500; t += 1) { 62 | const TargetState<1> target_state = model_ramp(otg.delta_time * t); 63 | const Result res = otg.update(target_state, input, output); 64 | std::cout << join(target_state.position) << " " << join(output.new_position) << std::endl; 65 | 66 | output.pass_to_input(input); 67 | } 68 | } 69 | -------------------------------------------------------------------------------- /examples/14_tracking.py: -------------------------------------------------------------------------------- 1 | # Only with Ruckig Pro 2 | 3 | from math import sin, cos 4 | 5 | from ruckig import Trackig, TargetState, InputParameter, OutputParameter 6 | 7 | 8 | # Create the target state signal 9 | def model_ramp(t, ramp_vel=0.5, ramp_pos=1.0): 10 | target = TargetState(1) 11 | on_ramp = t < ramp_pos / abs(ramp_vel) 12 | target.position = [t * ramp_vel] if on_ramp else [ramp_pos] 13 | target.velocity = [ramp_vel] if on_ramp else [0.0] 14 | target.acceleration = [0.0] 15 | return target 16 | 17 | 18 | def model_constant_acceleration(t, ramp_acc=0.05): 19 | target = TargetState(1) 20 | target.position = [t * t * ramp_acc] 21 | target.velocity = [t * ramp_acc] 22 | target.acceleration = [ramp_acc] 23 | return target 24 | 25 | 26 | def model_sinus(t, ramp_vel=0.4): 27 | target = TargetState(1) 28 | target.position = [sin(ramp_vel * t)] 29 | target.velocity = [ramp_vel * cos(ramp_vel * t)] 30 | target.acceleration = [-ramp_vel * ramp_vel * sin(ramp_vel * t)] 31 | return target 32 | 33 | 34 | if __name__ == '__main__': 35 | # Create instances: the Trackig OTG as well as input and output parameters 36 | inp = InputParameter(1) 37 | out = OutputParameter(inp.degrees_of_freedom) 38 | otg = Trackig(inp.degrees_of_freedom, 0.01) 39 | 40 | # Set input parameters 41 | inp.current_position = [0.0] 42 | inp.current_velocity = [0.0] 43 | inp.current_acceleration = [0.0] 44 | 45 | inp.max_velocity = [0.8] 46 | inp.max_acceleration = [2.0] 47 | inp.max_jerk = [5.0] 48 | 49 | # Optional minimum and maximum position 50 | inp.min_position = [-2.5] 51 | inp.max_position = [2.5] 52 | 53 | # otg.reactiveness = 1.0 # default value, should be in [0, 1] 54 | 55 | print('target | follow') 56 | 57 | # Generate the trajectory following the target state 58 | steps, target_list, follow_list = [], [], [] 59 | for t in range(500): 60 | target_state = model_ramp(otg.delta_time * t) 61 | 62 | steps.append(t) 63 | res = otg.update(target_state, inp, out) 64 | 65 | out.pass_to_input(inp) 66 | 67 | print('\t'.join([f'{p:0.3f}' for p in target_state.position] + [f'{p:0.3f}' for p in out.new_position]) + f' in {out.calculation_duration:0.2f} [µs]') 68 | 69 | target_list.append([target_state.position, target_state.velocity, target_state.acceleration]) 70 | follow_list.append([out.new_position, out.new_velocity, out.new_acceleration]) 71 | 72 | # Plot the trajectory 73 | # from pathlib import Path 74 | # project_path = Path(__file__).parent.parent.absolute() 75 | 76 | # import numpy as np 77 | # import matplotlib.pyplot as plt 78 | 79 | # follow_list = np.array(follow_list) 80 | # target_list = np.array(target_list) 81 | 82 | # plt.ylabel(f'DoF 1') 83 | # plt.plot(steps, follow_list[:, 0], label='Follow Position') 84 | # plt.plot(steps, follow_list[:, 1], label='Follow Velocity', linestyle='dotted') 85 | # plt.plot(steps, follow_list[:, 2], label='Follow Acceleration', linestyle='dotted') 86 | # plt.plot(steps, target_list[:, 0], color='r', label='Target Position') 87 | # plt.grid(True) 88 | # plt.legend() 89 | 90 | # plt.savefig(project_path / 'examples' / '13_trajectory.pdf') 91 | -------------------------------------------------------------------------------- /examples/14_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pantor/ruckig/a192afc8eb0834c28a2d9464933cb5acf5fc7669/examples/14_trajectory.pdf -------------------------------------------------------------------------------- /examples/CMakeLists-directory.txt: -------------------------------------------------------------------------------- 1 | # A CMakeLists.txt to show how to use Ruckig with CMake when library and headers are in a known directory. 2 | 3 | cmake_minimum_required(VERSION 3.10) 4 | 5 | project(ruckig_examples) 6 | 7 | include_directories(include) 8 | link_directories(lib) 9 | 10 | # Build the position example 11 | add_executable(example-position 01_position.cpp) 12 | target_compile_features(example-position PUBLIC cxx_std_17) 13 | 14 | target_link_libraries(example-position ruckig) -------------------------------------------------------------------------------- /examples/CMakeLists-installed.txt: -------------------------------------------------------------------------------- 1 | # A CMakeLists.txt to show how to use Ruckig with CMake when installed via `make install` first. 2 | 3 | cmake_minimum_required(VERSION 3.10) 4 | 5 | project(ruckig_examples) 6 | 7 | find_package(ruckig REQUIRED) 8 | 9 | # Build the position example 10 | add_executable(example-position 01_position.cpp) 11 | target_compile_features(example-position PUBLIC cxx_std_17) 12 | 13 | target_link_libraries(example-position PRIVATE ruckig::ruckig) -------------------------------------------------------------------------------- /examples/plotter.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | 6 | 7 | class Plotter: 8 | @staticmethod 9 | def plot_trajectory(filename, otg, inp, out_list, show=False, plot_acceleration=True, plot_jerk=True, time_offsets=None, title=None): 10 | taxis = np.array(list(map(lambda x: x.time, out_list))) 11 | if time_offsets: 12 | taxis += np.array(time_offsets) 13 | qaxis = np.array(list(map(lambda x: x.new_position, out_list))) 14 | dqaxis = np.array(list(map(lambda x: x.new_velocity, out_list))) 15 | ddqaxis = np.array(list(map(lambda x: x.new_acceleration, out_list))) 16 | dddqaxis = np.diff(ddqaxis, axis=0, prepend=ddqaxis[0, 0]) / otg.delta_time 17 | dddqaxis[0, :] = 0.0 18 | dddqaxis[-1, :] = 0.0 19 | 20 | plt.figure(figsize=(8.0, 2.0 + 3.0 * inp.degrees_of_freedom), dpi=120) 21 | plt.subplot(inp.degrees_of_freedom, 1, 1) 22 | 23 | if title: 24 | plt.title(title) 25 | 26 | for dof in range(inp.degrees_of_freedom): 27 | global_max = np.max([qaxis[:, dof], dqaxis[:, dof], ddqaxis[:, dof]]) 28 | global_min = np.min([qaxis[:, dof], dqaxis[:, dof], ddqaxis[:, dof]]) 29 | 30 | if plot_jerk: 31 | global_max = max(global_max, np.max(dddqaxis[:, dof])) 32 | global_min = min(global_min, np.min(dddqaxis[:, dof])) 33 | 34 | plt.subplot(inp.degrees_of_freedom, 1, dof + 1) 35 | plt.ylabel(f'DoF {dof + 1}') 36 | plt.plot(taxis, qaxis[:, dof], label=f'Position {dof + 1}') 37 | plt.plot(taxis, dqaxis[:, dof], label=f'Velocity {dof + 1}') 38 | if plot_acceleration: 39 | plt.plot(taxis, ddqaxis[:, dof], label=f'Acceleration {dof + 1}') 40 | if plot_jerk: 41 | plt.plot(taxis, dddqaxis[:, dof], label=f'Jerk {dof + 1}') 42 | 43 | # Plot sections 44 | if hasattr(out_list[-1], 'trajectory'): 45 | linewidth = 1.0 if len(out_list[-1].trajectory.intermediate_durations) < 20 else 0.25 46 | for t in out_list[-1].trajectory.intermediate_durations: 47 | plt.axvline(x=t, color='black', linestyle='--', linewidth=linewidth) 48 | 49 | # Plot limit lines 50 | if inp.min_position and inp.min_position[dof] > 1.4 * global_min: 51 | plt.axhline(y=inp.min_position[dof], color='grey', linestyle='--', linewidth=1.1) 52 | 53 | if inp.max_position and inp.max_position[dof] < 1.4 * global_max: 54 | plt.axhline(y=inp.max_position[dof], color='grey', linestyle='--', linewidth=1.1) 55 | 56 | if inp.max_velocity[dof] < 1.4 * global_max: 57 | plt.axhline(y=inp.max_velocity[dof], color='orange', linestyle='--', linewidth=1.1) 58 | 59 | min_velocity = inp.min_velocity[dof] if inp.min_velocity else -inp.max_velocity[dof] 60 | if min_velocity > 1.4 * global_min: 61 | plt.axhline(y=min_velocity, color='orange', linestyle='--', linewidth=1.1) 62 | 63 | if plot_acceleration and inp.max_acceleration[dof] < 1.4 * global_max: 64 | plt.axhline(y=inp.max_acceleration[dof], color='g', linestyle='--', linewidth=1.1) 65 | 66 | min_acceleration = inp.min_acceleration[dof] if inp.min_acceleration else -inp.max_acceleration[dof] 67 | if plot_acceleration and min_acceleration > 1.4 * global_min: 68 | plt.axhline(y=min_acceleration, color='g', linestyle='--', linewidth=1.1) 69 | 70 | if plot_jerk and inp.max_jerk[dof] < 1.4 * global_max: 71 | plt.axhline(y=inp.max_jerk[dof], color='red', linestyle='--', linewidth=1.1) 72 | 73 | if plot_jerk and -inp.max_jerk[dof] > 1.4 * global_min: 74 | plt.axhline(y=-inp.max_jerk[dof], color='red', linestyle='--', linewidth=1.1) 75 | 76 | plt.legend() 77 | plt.grid(True) 78 | 79 | plt.xlabel('t') 80 | plt.savefig(Path(__file__).parent.parent / 'build' / filename) 81 | 82 | if show: 83 | plt.show() 84 | -------------------------------------------------------------------------------- /include/ruckig/block.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | 12 | namespace ruckig { 13 | 14 | //! Which times are possible for synchronization? 15 | class Block { 16 | template 17 | inline static void remove_profile(std::array& valid_profiles, size_t& valid_profile_counter, size_t index) { 18 | for (size_t i = index; i < valid_profile_counter - 1; ++i) { 19 | valid_profiles[i] = valid_profiles[i + 1]; 20 | } 21 | valid_profile_counter -= 1; 22 | } 23 | 24 | public: 25 | struct Interval { 26 | double left, right; // [s] 27 | Profile profile; // Profile corresponding to right (end) time 28 | 29 | explicit Interval(double left, double right): left(left), right(right) { } 30 | 31 | explicit Interval(const Profile& profile_left, const Profile& profile_right) { 32 | const double left_duration = profile_left.t_sum.back() + profile_left.brake.duration + profile_left.accel.duration; 33 | const double right_duration = profile_right.t_sum.back() + profile_right.brake.duration + profile_right.accel.duration; 34 | if (left_duration < right_duration) { 35 | left = left_duration; 36 | right = right_duration; 37 | profile = profile_right; 38 | } else { 39 | left = right_duration; 40 | right = left_duration; 41 | profile = profile_left; 42 | } 43 | }; 44 | }; 45 | 46 | void set_min_profile(const Profile& profile) { 47 | p_min = profile; 48 | t_min = p_min.t_sum.back() + p_min.brake.duration + p_min.accel.duration; 49 | a = std::nullopt; 50 | b = std::nullopt; 51 | } 52 | 53 | Profile p_min; // Save min profile so that it doesn't need to be recalculated in Step2 54 | double t_min; // [s] 55 | 56 | // Max. 2 intervals can be blocked: called a and b with corresponding profiles, order does not matter 57 | std::optional a, b; 58 | 59 | template 60 | static bool calculate_block(Block& block, std::array& valid_profiles, size_t valid_profile_counter) { 61 | // std::cout << "---\n " << valid_profile_counter << std::endl; 62 | // for (size_t i = 0; i < valid_profile_counter; ++i) { 63 | // std::cout << valid_profiles[i].t_sum.back() << " " << valid_profiles[i].to_string() << std::endl; 64 | // } 65 | 66 | if (valid_profile_counter == 1) { 67 | block.set_min_profile(valid_profiles[0]); 68 | return true; 69 | 70 | } else if (valid_profile_counter == 2) { 71 | if (std::abs(valid_profiles[0].t_sum.back() - valid_profiles[1].t_sum.back()) < 8 * std::numeric_limits::epsilon()) { 72 | block.set_min_profile(valid_profiles[0]); 73 | return true; 74 | } 75 | 76 | if constexpr (numerical_robust) { 77 | const size_t idx_min = (valid_profiles[0].t_sum.back() < valid_profiles[1].t_sum.back()) ? 0 : 1; 78 | const size_t idx_else_1 = (idx_min + 1) % 2; 79 | 80 | block.set_min_profile(valid_profiles[idx_min]); 81 | block.a = Interval(valid_profiles[idx_min], valid_profiles[idx_else_1]); 82 | return true; 83 | } 84 | 85 | // Only happens due to numerical issues 86 | } else if (valid_profile_counter == 4) { 87 | // Find "identical" profiles 88 | if (std::abs(valid_profiles[0].t_sum.back() - valid_profiles[1].t_sum.back()) < 32 * std::numeric_limits::epsilon() && valid_profiles[0].direction != valid_profiles[1].direction) { 89 | remove_profile(valid_profiles, valid_profile_counter, 1); 90 | } else if (std::abs(valid_profiles[2].t_sum.back() - valid_profiles[3].t_sum.back()) < 256 * std::numeric_limits::epsilon() && valid_profiles[2].direction != valid_profiles[3].direction) { 91 | remove_profile(valid_profiles, valid_profile_counter, 3); 92 | } else if (std::abs(valid_profiles[0].t_sum.back() - valid_profiles[3].t_sum.back()) < 256 * std::numeric_limits::epsilon() && valid_profiles[0].direction != valid_profiles[3].direction) { 93 | remove_profile(valid_profiles, valid_profile_counter, 3); 94 | } else { 95 | return false; 96 | } 97 | 98 | } else if (valid_profile_counter % 2 == 0) { 99 | return false; 100 | } 101 | 102 | // Find index of fastest profile 103 | const auto idx_min_it = std::min_element(valid_profiles.cbegin(), valid_profiles.cbegin() + valid_profile_counter, [](const Profile& a, const Profile& b) { return a.t_sum.back() < b.t_sum.back(); }); 104 | const size_t idx_min = std::distance(valid_profiles.cbegin(), idx_min_it); 105 | 106 | block.set_min_profile(valid_profiles[idx_min]); 107 | 108 | if (valid_profile_counter == 3) { 109 | const size_t idx_else_1 = (idx_min + 1) % 3; 110 | const size_t idx_else_2 = (idx_min + 2) % 3; 111 | 112 | block.a = Interval(valid_profiles[idx_else_1], valid_profiles[idx_else_2]); 113 | return true; 114 | 115 | } else if (valid_profile_counter == 5) { 116 | const size_t idx_else_1 = (idx_min + 1) % 5; 117 | const size_t idx_else_2 = (idx_min + 2) % 5; 118 | const size_t idx_else_3 = (idx_min + 3) % 5; 119 | const size_t idx_else_4 = (idx_min + 4) % 5; 120 | 121 | if (valid_profiles[idx_else_1].direction == valid_profiles[idx_else_2].direction) { 122 | block.a = Interval(valid_profiles[idx_else_1], valid_profiles[idx_else_2]); 123 | block.b = Interval(valid_profiles[idx_else_3], valid_profiles[idx_else_4]); 124 | } else { 125 | block.a = Interval(valid_profiles[idx_else_1], valid_profiles[idx_else_4]); 126 | block.b = Interval(valid_profiles[idx_else_2], valid_profiles[idx_else_3]); 127 | } 128 | return true; 129 | } 130 | 131 | return false; 132 | } 133 | 134 | inline bool is_blocked(double t) const { 135 | return (t < t_min) || (a && a->left < t && t < a->right) || (b && b->left < t && t < b->right); 136 | } 137 | 138 | const Profile& get_profile(double t) const { 139 | if (b && t >= b->right) { 140 | return b->profile; 141 | } 142 | if (a && t >= a->right) { 143 | return a->profile; 144 | } 145 | return p_min; 146 | } 147 | 148 | std::string to_string() const { 149 | std::string result = "[" + std::to_string(t_min) + " "; 150 | if (a) { 151 | result += std::to_string(a->left) + "] [" + std::to_string(a->right) + " "; 152 | } 153 | if (b) { 154 | result += std::to_string(b->left) + "] [" + std::to_string(b->right) + " "; 155 | } 156 | return result + "-"; 157 | } 158 | }; 159 | 160 | } // namespace ruckig 161 | -------------------------------------------------------------------------------- /include/ruckig/brake.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | 10 | namespace ruckig { 11 | 12 | //! Calculates (pre- or post-) profile to get current or final state below the limits 13 | class BrakeProfile { 14 | static constexpr double eps {2.2e-14}; 15 | 16 | void acceleration_brake(double v0, double a0, double vMax, double vMin, double aMax, double aMin, double jMax); 17 | void velocity_brake(double v0, double a0, double vMax, double vMin, double aMax, double aMin, double jMax); 18 | 19 | public: 20 | //! Overall duration 21 | double duration {0.0}; 22 | 23 | //! Profile information for a two-step profile 24 | std::array t, j, a, v, p; 25 | 26 | //! Calculate brake trajectory for third-order position interface 27 | void get_position_brake_trajectory(double v0, double a0, double vMax, double vMin, double aMax, double aMin, double jMax); 28 | 29 | //! Calculate brake trajectory for second-order position interface 30 | void get_second_order_position_brake_trajectory(double v0, double vMax, double vMin, double aMax, double aMin); 31 | 32 | //! Calculate brake trajectory for third-order velocity interface 33 | void get_velocity_brake_trajectory(double a0, double aMax, double aMin, double jMax); 34 | 35 | //! Calculate brake trajectory for second-order velocity interface 36 | void get_second_order_velocity_brake_trajectory(); 37 | 38 | //! Finalize third-order braking by integrating along kinematic state 39 | void finalize(double& ps, double& vs, double& as) { 40 | if (t[0] <= 0.0 && t[1] <= 0.0) { 41 | duration = 0.0; 42 | return; 43 | } 44 | 45 | duration = t[0]; 46 | p[0] = ps; 47 | v[0] = vs; 48 | a[0] = as; 49 | std::tie(ps, vs, as) = integrate(t[0], ps, vs, as, j[0]); 50 | 51 | if (t[1] > 0.0) { 52 | duration += t[1]; 53 | p[1] = ps; 54 | v[1] = vs; 55 | a[1] = as; 56 | std::tie(ps, vs, as) = integrate(t[1], ps, vs, as, j[1]); 57 | } 58 | } 59 | 60 | //! Finalize second-order braking by integrating along kinematic state 61 | void finalize_second_order(double& ps, double& vs, double& as) { 62 | if (t[0] <= 0.0) { 63 | duration = 0.0; 64 | return; 65 | } 66 | 67 | duration = t[0]; 68 | p[0] = ps; 69 | v[0] = vs; 70 | std::tie(ps, vs, as) = integrate(t[0], ps, vs, a[0], 0.0); 71 | } 72 | }; 73 | 74 | } // namespace ruckig 75 | -------------------------------------------------------------------------------- /include/ruckig/calculator.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #ifdef WITH_CLOUD_CLIENT 5 | #include 6 | #endif 7 | #include 8 | #include 9 | 10 | 11 | namespace ruckig { 12 | 13 | //! Internal interface for the main calculator and its hyperparameters 14 | template class CustomVector = StandardVector> 15 | class Calculator { 16 | inline bool use_waypoints_trajectory(const InputParameter& input) { 17 | return !input.intermediate_positions.empty() && input.control_interface == ControlInterface::Position; 18 | } 19 | 20 | public: 21 | //! Calculator for state-to-state trajectories 22 | TargetCalculator target_calculator; 23 | 24 | #if defined WITH_CLOUD_CLIENT 25 | //! Calculator for trajectories with intermediate waypoints 26 | WaypointsCalculator waypoints_calculator; 27 | #endif 28 | 29 | template= 1), int>::type = 0> 30 | explicit Calculator() { } 31 | 32 | #if defined WITH_CLOUD_CLIENT 33 | template= 1), int>::type = 0> 34 | explicit Calculator(size_t max_waypoints): 35 | waypoints_calculator(WaypointsCalculator(max_waypoints)) 36 | { } 37 | 38 | template::type = 0> 39 | explicit Calculator(size_t dofs): 40 | target_calculator(TargetCalculator(dofs)), 41 | waypoints_calculator(WaypointsCalculator(dofs)) 42 | { } 43 | 44 | template::type = 0> 45 | explicit Calculator(size_t dofs, size_t max_waypoints): 46 | target_calculator(TargetCalculator(dofs)), 47 | waypoints_calculator(WaypointsCalculator(dofs, max_waypoints)) 48 | { } 49 | #else 50 | template::type = 0> 51 | explicit Calculator(size_t dofs): target_calculator(TargetCalculator(dofs)) { } 52 | #endif 53 | 54 | //! Calculate the time-optimal waypoint-based trajectory 55 | template 56 | Result calculate(const InputParameter& input, Trajectory& trajectory, double delta_time, bool& was_interrupted) { 57 | Result result; 58 | #if defined WITH_CLOUD_CLIENT 59 | if (use_waypoints_trajectory(input)) { 60 | result = waypoints_calculator.template calculate(input, trajectory, delta_time, was_interrupted); 61 | } else { 62 | result = target_calculator.template calculate(input, trajectory, delta_time, was_interrupted); 63 | } 64 | #else 65 | result = target_calculator.template calculate(input, trajectory, delta_time, was_interrupted); 66 | #endif 67 | 68 | return result; 69 | } 70 | 71 | //! Continue the trajectory calculation 72 | template 73 | Result continue_calculation(const InputParameter& input, Trajectory& trajectory, double delta_time, bool& was_interrupted) { 74 | Result result; 75 | #if defined WITH_CLOUD_CLIENT 76 | if (use_waypoints_trajectory(input)) { 77 | result = waypoints_calculator.template continue_calculation(input, trajectory, delta_time, was_interrupted); 78 | } else { 79 | result = target_calculator.template continue_calculation(input, trajectory, delta_time, was_interrupted); 80 | } 81 | #else 82 | result = target_calculator.template continue_calculation(input, trajectory, delta_time, was_interrupted); 83 | #endif 84 | 85 | return result; 86 | } 87 | }; 88 | 89 | } // namespace ruckig 90 | -------------------------------------------------------------------------------- /include/ruckig/calculator_cloud.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | 20 | namespace httplib { class Client; } 21 | 22 | namespace ruckig { 23 | 24 | class CloudClient { 25 | std::shared_ptr cli; 26 | 27 | public: 28 | explicit CloudClient(); 29 | 30 | nlohmann::json post(const nlohmann::json& params, bool throw_error); 31 | }; 32 | 33 | 34 | NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(BrakeProfile, duration, t, j, a, v, p) 35 | NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(Profile, t, t_sum, j, a, v, p, pf, vf, af, brake, accel) 36 | 37 | 38 | //! Calculation class for a trajectory along waypoints. 39 | template class CustomVector = StandardVector> 40 | class WaypointsCalculator { 41 | template using Vector = CustomVector; 42 | 43 | // Convert the custom MinimumVector type to json, using only .size() and [] 44 | template 45 | static void vector_to_json(nlohmann::json& j, const V& vector) { 46 | j = nlohmann::json::array(); 47 | auto& j_vector = j.get_ref(); 48 | 49 | j_vector.resize(vector.size()); 50 | for (size_t i = 0; i < vector.size(); ++i) { 51 | j_vector[i] = vector[i]; 52 | } 53 | } 54 | 55 | // Convert a double vector to json 56 | template 57 | static void double_vector_to_json(nlohmann::json& j, const std::vector& vector) { 58 | j = nlohmann::json::array(); 59 | auto& j_vector = j.get_ref(); 60 | 61 | j_vector.resize(vector.size()); 62 | for (size_t i = 0; i < vector.size(); ++i) { 63 | vector_to_json(j_vector[i], vector[i]); 64 | } 65 | } 66 | 67 | CloudClient client; 68 | 69 | public: 70 | size_t degrees_of_freedom; 71 | 72 | template= 1), int>::type = 0> 73 | explicit WaypointsCalculator(): degrees_of_freedom(DOFs) { } 74 | 75 | template= 1), int>::type = 0> 76 | explicit WaypointsCalculator(size_t): degrees_of_freedom(DOFs) { } 77 | 78 | template::type = 0> 79 | explicit WaypointsCalculator(size_t dofs): degrees_of_freedom(dofs) { } 80 | 81 | template::type = 0> 82 | explicit WaypointsCalculator(size_t dofs, size_t): degrees_of_freedom(dofs) { } 83 | 84 | template 85 | Result calculate(const InputParameter& input, Trajectory& traj, double, bool& was_interrupted) { 86 | std::cout << "[ruckig] calculate trajectory via cloud API." << std::endl; 87 | 88 | nlohmann::json params; 89 | params["degrees_of_freedom"] = input.degrees_of_freedom; 90 | vector_to_json>(params["current_position"], input.current_position); 91 | vector_to_json>(params["current_velocity"], input.current_velocity); 92 | vector_to_json>(params["current_acceleration"], input.current_acceleration); 93 | vector_to_json>(params["target_position"], input.target_position); 94 | vector_to_json>(params["target_velocity"], input.target_velocity); 95 | vector_to_json>(params["target_acceleration"], input.target_acceleration); 96 | vector_to_json>(params["max_velocity"], input.max_velocity); 97 | vector_to_json>(params["max_acceleration"], input.max_acceleration); 98 | vector_to_json>(params["max_jerk"], input.max_jerk); 99 | if (input.min_velocity) { 100 | vector_to_json>(params["min_velocity"], input.min_velocity.value()); 101 | } 102 | if (input.min_acceleration) { 103 | vector_to_json>(params["min_acceleration"], input.min_acceleration.value()); 104 | } 105 | if (!input.intermediate_positions.empty()) { 106 | double_vector_to_json>(params["intermediate_positions"], input.intermediate_positions); 107 | } 108 | if (input.per_section_max_velocity) { 109 | double_vector_to_json>(params["per_section_max_velocity"], input.per_section_max_velocity.value()); 110 | } 111 | if (input.per_section_max_acceleration) { 112 | double_vector_to_json>(params["per_section_max_acceleration"], input.per_section_max_acceleration.value()); 113 | } 114 | if (input.per_section_max_jerk) { 115 | double_vector_to_json>(params["per_section_max_jerk"], input.per_section_max_jerk.value()); 116 | } 117 | if (input.per_section_min_velocity) { 118 | double_vector_to_json>(params["per_section_min_velocity"], input.per_section_min_velocity.value()); 119 | } 120 | if (input.per_section_min_acceleration) { 121 | double_vector_to_json>(params["per_section_min_acceleration"], input.per_section_min_acceleration.value()); 122 | } 123 | if (input.max_position) { 124 | vector_to_json>(params["max_position"], input.max_position.value()); 125 | } 126 | if (input.min_position) { 127 | vector_to_json>(params["min_position"], input.min_position.value()); 128 | } 129 | vector_to_json>(params["enabled"], input.enabled); 130 | params["control_interface"] = input.control_interface; 131 | params["synchronization"] = input.synchronization; 132 | params["duration_discretization"] = input.duration_discretization; 133 | if (input.per_dof_control_interface) { 134 | vector_to_json>(params["per_dof_control_interface"], input.per_dof_control_interface.value()); 135 | } 136 | if (input.per_dof_synchronization) { 137 | vector_to_json>(params["per_dof_synchronization"], input.per_dof_synchronization.value()); 138 | } 139 | if (input.minimum_duration) { 140 | params["minimum_duration"] = input.minimum_duration.value(); 141 | } 142 | if (input.per_section_minimum_duration) { 143 | vector_to_json>(params["per_section_minimum_duration"], input.per_section_minimum_duration.value()); 144 | } 145 | 146 | const auto result = client.post(params, throw_error); 147 | 148 | was_interrupted = false; 149 | traj.degrees_of_freedom = input.degrees_of_freedom; 150 | traj.resize(result["profiles"].size() - 1); 151 | 152 | traj.continue_calculation_counter = 0; 153 | traj.duration = result["duration"].template get(); 154 | traj.cumulative_times = result["cumulative_times"].template get>(); 155 | 156 | if (!result["message"].empty()) { 157 | std::cout << "[ruckig] " << result["message"] << std::endl; 158 | } 159 | 160 | if (result["result"] == "Result.Error") { 161 | return Result::Error; 162 | 163 | } else if (result["result"] == "Result.ErrorInvalidInput") { 164 | return Result::ErrorInvalidInput; 165 | } 166 | 167 | for (size_t i = 0; i < result["profiles"].size(); ++i) { 168 | for (size_t dof = 0; dof < traj.degrees_of_freedom; ++dof) { 169 | auto& p = result["profiles"][i][dof]; 170 | traj.profiles[i][dof] = p.template get(); 171 | } 172 | } 173 | 174 | return Result::Working; 175 | } 176 | 177 | template 178 | Result continue_calculation(const InputParameter&, Trajectory&, double, bool&) { 179 | if constexpr (throw_error) { 180 | throw RuckigError("continue calculation not available in Ruckig Community Version."); 181 | } else { 182 | return Result::Error; 183 | } 184 | } 185 | }; 186 | 187 | } // namespace ruckig_pro 188 | -------------------------------------------------------------------------------- /include/ruckig/error.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | namespace ruckig { 9 | 10 | //! The base class for all exceptions 11 | struct RuckigError: public std::runtime_error { 12 | explicit RuckigError(const std::string& message): std::runtime_error("\n[ruckig] " + message) { } 13 | }; 14 | 15 | } 16 | -------------------------------------------------------------------------------- /include/ruckig/output_parameter.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | 11 | namespace ruckig { 12 | 13 | //! Output of the Ruckig algorithm 14 | template class CustomVector = StandardVector> 15 | class OutputParameter { 16 | template using Vector = CustomVector; 17 | 18 | void resize(size_t dofs) { 19 | new_position.resize(dofs); 20 | new_velocity.resize(dofs); 21 | new_acceleration.resize(dofs); 22 | new_jerk.resize(dofs); 23 | } 24 | 25 | public: 26 | size_t degrees_of_freedom; 27 | 28 | //! Current trajectory 29 | Trajectory trajectory; 30 | 31 | // Current kinematic state 32 | Vector new_position, new_velocity, new_acceleration, new_jerk; 33 | 34 | //! Current time on trajectory 35 | double time {0.0}; 36 | 37 | //! Index of the current section between two (possibly filtered) intermediate positions (only relevant in Ruckig Pro) 38 | size_t new_section {0}; 39 | 40 | //! Was a new section reached in the last cycle? (only relevant in Ruckig Pro) 41 | bool did_section_change {false}; 42 | 43 | //! Was a new trajectory calculation performed in the last cycle? 44 | bool new_calculation {false}; 45 | 46 | //! Was the trajectory calculation interrupted? (only in Ruckig Pro) 47 | bool was_calculation_interrupted {false}; 48 | 49 | //! Computational duration of the last update call 50 | double calculation_duration; // [µs] 51 | 52 | template= 1), int>::type = 0> 53 | OutputParameter(): degrees_of_freedom(DOFs) { } 54 | 55 | template::type = 0> 56 | OutputParameter(size_t dofs): 57 | degrees_of_freedom(dofs), 58 | trajectory(Trajectory<0, CustomVector>(dofs)) 59 | { 60 | resize(dofs); 61 | } 62 | 63 | #if defined WITH_CLOUD_CLIENT 64 | template= 1), int>::type = 0> 65 | OutputParameter(size_t max_number_of_waypoints): 66 | degrees_of_freedom(DOFs), 67 | trajectory(Trajectory(max_number_of_waypoints)) 68 | { } 69 | 70 | template::type = 0> 71 | OutputParameter(size_t dofs, size_t max_number_of_waypoints): 72 | degrees_of_freedom(dofs), 73 | trajectory(Trajectory<0, CustomVector>(dofs, max_number_of_waypoints)) 74 | { 75 | resize(dofs); 76 | } 77 | #endif 78 | 79 | void pass_to_input(InputParameter& input) const { 80 | input.current_position = new_position; 81 | input.current_velocity = new_velocity; 82 | input.current_acceleration = new_acceleration; 83 | 84 | // Remove first intermediate waypoint if section did change 85 | if (did_section_change && !input.intermediate_positions.empty()) { 86 | input.intermediate_positions.erase(input.intermediate_positions.begin()); 87 | } 88 | } 89 | 90 | std::string to_string() const { 91 | std::stringstream ss; 92 | ss << "\nout.new_position = [" << join(new_position, true) << "]\n"; 93 | ss << "out.new_velocity = [" << join(new_velocity, true) << "]\n"; 94 | ss << "out.new_acceleration = [" << join(new_acceleration, true) << "]\n"; 95 | ss << "out.new_jerk = [" << join(new_jerk, true) << "]\n"; 96 | ss << "out.time = [" << std::setprecision(16) << time << "]\n"; 97 | ss << "out.calculation_duration = [" << std::setprecision(16) << calculation_duration << "]\n"; 98 | return ss.str(); 99 | } 100 | }; 101 | 102 | } // namespace ruckig 103 | -------------------------------------------------------------------------------- /include/ruckig/position.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | 7 | namespace ruckig { 8 | 9 | 10 | 11 | //! Mathematical equations for Step 1 in third-order position interface: Extremal profiles 12 | class PositionThirdOrderStep1 { 13 | using ReachedLimits = Profile::ReachedLimits; 14 | using ControlSigns = Profile::ControlSigns; 15 | 16 | const double v0, a0; 17 | const double vf, af; 18 | const double _vMax, _vMin, _aMax, _aMin, _jMax; 19 | 20 | // Pre-calculated expressions 21 | double pd; 22 | double v0_v0, vf_vf; 23 | double a0_a0, a0_p3, a0_p4; 24 | double af_af, af_p3, af_p4; 25 | double jMax_jMax; 26 | 27 | // Max 5 valid profiles + 1 spare for numerical issues 28 | using ProfileIter = std::array::iterator; 29 | std::array valid_profiles; 30 | 31 | void time_all_vel(ProfileIter& profile, double vMax, double vMin, double aMax, double aMin, double jMax, bool return_after_found) const; 32 | void time_acc0_acc1(ProfileIter& profile, double vMax, double vMin, double aMax, double aMin, double jMax, bool return_after_found) const; 33 | void time_all_none_acc0_acc1(ProfileIter& profile, double vMax, double vMin, double aMax, double aMin, double jMax, bool return_after_found) const; 34 | 35 | // Only for numerical issues, always return_after_found 36 | void time_acc1_vel_two_step(ProfileIter& profile, double vMax, double vMin, double aMax, double aMin, double jMax) const; 37 | void time_acc0_two_step(ProfileIter& profile, double vMax, double vMin, double aMax, double aMin, double jMax) const; 38 | void time_vel_two_step(ProfileIter& profile, double vMax, double vMin, double aMax, double aMin, double jMax) const; 39 | void time_none_two_step(ProfileIter& profile, double vMax, double vMin, double aMax, double aMin, double jMax) const; 40 | 41 | // Only for zero-limits case 42 | bool time_all_single_step(Profile* profile, double vMax, double vMin, double aMax, double aMin, double jMax) const; 43 | 44 | inline void add_profile(ProfileIter& profile) const { 45 | const auto prev_profile = profile; 46 | ++profile; 47 | profile->set_boundary(*prev_profile); 48 | } 49 | 50 | public: 51 | explicit PositionThirdOrderStep1(double p0, double v0, double a0, double pf, double vf, double af, double vMax, double vMin, double aMax, double aMin, double jMax); 52 | 53 | bool get_profile(const Profile& input, Block& block); 54 | }; 55 | 56 | 57 | //! Mathematical equations for Step 2 in third-order position interface: Time synchronization 58 | class PositionThirdOrderStep2 { 59 | using ReachedLimits = Profile::ReachedLimits; 60 | using ControlSigns = Profile::ControlSigns; 61 | 62 | const double v0, a0; 63 | const double tf, vf, af; 64 | const double _vMax, _vMin, _aMax, _aMin, _jMax; 65 | 66 | // Pre-calculated expressions 67 | double pd; 68 | double tf_tf, tf_p3, tf_p4; 69 | double vd, vd_vd; 70 | double ad, ad_ad; 71 | double v0_v0, vf_vf; 72 | double a0_a0, a0_p3, a0_p4, a0_p5, a0_p6; 73 | double af_af, af_p3, af_p4, af_p5, af_p6; 74 | double jMax_jMax; 75 | double g1, g2; 76 | 77 | bool time_acc0_acc1_vel(Profile& profile, double vMax, double vMin, double aMax, double aMin, double jMax); 78 | bool time_acc1_vel(Profile& profile, double vMax, double vMin, double aMax, double aMin, double jMax); 79 | bool time_acc0_vel(Profile& profile, double vMax, double vMin, double aMax, double aMin, double jMax); 80 | bool time_vel(Profile& profile, double vMax, double vMin, double aMax, double aMin, double jMax); 81 | bool time_acc0_acc1(Profile& profile, double vMax, double vMin, double aMax, double aMin, double jMax); 82 | bool time_acc1(Profile& profile, double vMax, double vMin, double aMax, double aMin, double jMax); 83 | bool time_acc0(Profile& profile, double vMax, double vMin, double aMax, double aMin, double jMax); 84 | bool time_none(Profile& profile, double vMax, double vMin, double aMax, double aMin, double jMax); 85 | bool time_none_smooth(Profile& profile, double vMax, double vMin, double aMax, double aMin, double jMax); 86 | 87 | public: 88 | bool minimize_jerk {false}; 89 | 90 | explicit PositionThirdOrderStep2(double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double vMin, double aMax, double aMin, double jMax); 91 | 92 | bool get_profile(Profile& profile); 93 | }; 94 | 95 | 96 | //! Mathematical equations for Step 1 in second-order position interface: Extremal profiles 97 | class PositionSecondOrderStep1 { 98 | using ReachedLimits = Profile::ReachedLimits; 99 | using ControlSigns = Profile::ControlSigns; 100 | 101 | const double v0, vf; 102 | const double _vMax, _vMin, _aMax, _aMin; 103 | 104 | // Pre-calculated expressions 105 | double pd; 106 | 107 | // Max 3 valid profiles 108 | using ProfileIter = std::array::iterator; 109 | std::array valid_profiles; 110 | 111 | void time_acc0(ProfileIter& profile, double vMax, double vMin, double aMax, double aMin, bool return_after_found) const; 112 | void time_none(ProfileIter& profile, double vMax, double vMin, double aMax, double aMin, bool return_after_found) const; 113 | 114 | // Only for zero-limits case 115 | bool time_all_single_step(Profile* profile, double vMax, double vMin, double aMax, double aMin) const; 116 | 117 | inline void add_profile(ProfileIter& profile) const { 118 | const auto prev_profile = profile; 119 | ++profile; 120 | profile->set_boundary(*prev_profile); 121 | } 122 | 123 | public: 124 | explicit PositionSecondOrderStep1(double p0, double v0, double pf, double vf, double vMax, double vMin, double aMax, double aMin); 125 | 126 | bool get_profile(const Profile& input, Block& block); 127 | }; 128 | 129 | 130 | //! Mathematical equations for Step 2 in second-order position interface: Time synchronization 131 | class PositionSecondOrderStep2 { 132 | using ReachedLimits = Profile::ReachedLimits; 133 | using ControlSigns = Profile::ControlSigns; 134 | 135 | const double v0, tf, vf; 136 | const double _vMax, _vMin, _aMax, _aMin; 137 | 138 | // Pre-calculated expressions 139 | double pd, vd; 140 | 141 | bool time_acc0(Profile& profile, double vMax, double vMin, double aMax, double aMin); 142 | bool time_none(Profile& profile, double vMax, double vMin, double aMax, double aMin); 143 | 144 | inline bool check_all(Profile& profile, double vMax, double vMin, double aMax, double aMin) { 145 | return time_acc0(profile, vMax, vMin, aMax, aMin) || time_none(profile, vMax, vMin, aMax, aMin); 146 | } 147 | 148 | public: 149 | explicit PositionSecondOrderStep2(double tf, double p0, double v0, double pf, double vf, double vMax, double vMin, double aMax, double aMin); 150 | 151 | bool get_profile(Profile& profile); 152 | }; 153 | 154 | 155 | //! Mathematical equations for Step 1 in first-order position interface: Extremal profiles 156 | class PositionFirstOrderStep1 { 157 | const double _vMax, _vMin; 158 | double pd; // Pre-calculated expressions 159 | 160 | public: 161 | explicit PositionFirstOrderStep1(double p0, double pf, double vMax, double vMin); 162 | 163 | bool get_profile(const Profile& input, Block& block); 164 | }; 165 | 166 | 167 | //! Mathematical equations for Step 2 in first-order position interface: Time synchronization 168 | class PositionFirstOrderStep2 { 169 | const double tf, _vMax, _vMin; 170 | double pd; // Pre-calculated expressions 171 | 172 | public: 173 | explicit PositionFirstOrderStep2(double tf, double p0, double pf, double vMax, double vMin); 174 | 175 | bool get_profile(Profile& profile); 176 | }; 177 | 178 | } // namespace ruckig 179 | -------------------------------------------------------------------------------- /include/ruckig/result.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | 4 | namespace ruckig { 5 | 6 | //! Result type of Ruckig's update function 7 | enum Result { 8 | Working = 0, ///< The trajectory is calculated normally 9 | Finished = 1, ///< The trajectory has reached its final position 10 | Error = -1, ///< Unclassified error 11 | ErrorInvalidInput = -100, ///< Error in the input parameter 12 | ErrorTrajectoryDuration = -101, ///< The trajectory duration exceeds its numerical limits 13 | ErrorPositionalLimits = -102, ///< The trajectory exceeds the given positional limits (only in Ruckig Pro) 14 | // ErrorNoPhaseSynchronization = -103, ///< The trajectory cannot be phase synchronized 15 | ErrorZeroLimits = -104, ///< The trajectory is not valid due to a conflict with zero limits 16 | ErrorExecutionTimeCalculation = -110, ///< Error during the extremel time calculation (Step 1) 17 | ErrorSynchronizationCalculation = -111, ///< Error during the synchronization calculation (Step 2) 18 | }; 19 | 20 | } 21 | -------------------------------------------------------------------------------- /include/ruckig/roots.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | namespace ruckig { 10 | 11 | template 12 | inline T pow2(T v) { 13 | return v * v; 14 | } 15 | 16 | 17 | namespace roots { 18 | 19 | // Use own set class on stack for real-time capability 20 | template 21 | class Set { 22 | protected: 23 | using Container = typename std::array; 24 | using iterator = typename Container::iterator; 25 | 26 | Container data; 27 | size_t size {0}; 28 | 29 | public: 30 | // Sort when accessing the elements 31 | const iterator begin() { 32 | std::sort(data.begin(), data.begin() + size); 33 | return data.begin(); 34 | } 35 | 36 | const iterator end() { 37 | return data.begin() + size; 38 | } 39 | 40 | void insert(T value) { 41 | data[size] = value; 42 | ++size; 43 | } 44 | }; 45 | 46 | 47 | // Set that only inserts positive values 48 | template 49 | class PositiveSet: public Set { 50 | public: 51 | void insert(T value) { 52 | if (value >= 0) { 53 | Set::insert(value); 54 | } 55 | } 56 | }; 57 | 58 | 59 | //! Calculate all roots of a*x^3 + b*x^2 + c*x + d = 0 60 | inline PositiveSet solve_cubic(double a, double b, double c, double d) { 61 | PositiveSet roots; 62 | 63 | if (std::abs(d) < DBL_EPSILON) { 64 | // First solution is x = 0 65 | roots.insert(0.0); 66 | 67 | // Converting to a quadratic equation 68 | d = c; 69 | c = b; 70 | b = a; 71 | a = 0.0; 72 | } 73 | 74 | if (std::abs(a) < DBL_EPSILON) { 75 | if (std::abs(b) < DBL_EPSILON) { 76 | // Linear equation 77 | if (std::abs(c) > DBL_EPSILON) { 78 | roots.insert(-d / c); 79 | } 80 | 81 | } else { 82 | // Quadratic equation 83 | const double discriminant = c * c - 4 * b * d; 84 | if (discriminant >= 0) { 85 | const double inv2b = 1.0 / (2 * b); 86 | const double y = std::sqrt(discriminant); 87 | roots.insert((-c + y) * inv2b); 88 | roots.insert((-c - y) * inv2b); 89 | } 90 | } 91 | 92 | } else { 93 | // Cubic equation 94 | const double inva = 1.0 / a; 95 | const double invaa = inva * inva; 96 | const double bb = b * b; 97 | const double bover3a = b * inva / 3; 98 | const double p = (a * c - bb / 3) * invaa; 99 | const double halfq = (2 * bb * b - 9 * a * b * c + 27 * a * a * d) / 54 * invaa * inva; 100 | const double yy = p * p * p / 27 + halfq * halfq; 101 | 102 | constexpr double cos120 = -0.50; 103 | constexpr double sin120 = 0.866025403784438646764; 104 | 105 | if (yy > DBL_EPSILON) { 106 | // Sqrt is positive: one real solution 107 | const double y = std::sqrt(yy); 108 | const double uuu = -halfq + y; 109 | const double vvv = -halfq - y; 110 | const double www = std::abs(uuu) > std::abs(vvv) ? uuu : vvv; 111 | const double w = std::cbrt(www); 112 | roots.insert(w - p / (3 * w) - bover3a); 113 | } else if (yy < -DBL_EPSILON) { 114 | // Sqrt is negative: three real solutions 115 | const double x = -halfq; 116 | const double y = std::sqrt(-yy); 117 | double theta; 118 | double r; 119 | 120 | // Convert to polar form 121 | if (std::abs(x) > DBL_EPSILON) { 122 | theta = (x > 0.0) ? std::atan(y / x) : (std::atan(y / x) + M_PI); 123 | r = std::sqrt(x * x - yy); 124 | } else { 125 | // Vertical line 126 | theta = M_PI / 2; 127 | r = y; 128 | } 129 | // Calculate cube root 130 | theta /= 3; 131 | r = 2 * std::cbrt(r); 132 | // Convert to complex coordinate 133 | const double ux = std::cos(theta) * r; 134 | const double uyi = std::sin(theta) * r; 135 | 136 | roots.insert(ux - bover3a); 137 | roots.insert(ux * cos120 - uyi * sin120 - bover3a); 138 | roots.insert(ux * cos120 + uyi * sin120 - bover3a); 139 | } else { 140 | // Sqrt is zero: two real solutions 141 | const double www = -halfq; 142 | const double w = 2 * std::cbrt(www); 143 | 144 | roots.insert(w - bover3a); 145 | roots.insert(w * cos120 - bover3a); 146 | } 147 | } 148 | return roots; 149 | } 150 | 151 | // Solve resolvent eqaution of corresponding Quartic equation 152 | // The input x must be of length 3 153 | // Number of zeros are returned 154 | inline int solve_resolvent(std::array& x, double a, double b, double c) { 155 | constexpr double cos120 = -0.50; 156 | constexpr double sin120 = 0.866025403784438646764; 157 | 158 | a /= 3; 159 | const double a2 = a * a; 160 | double q = a2 - b / 3; 161 | const double r = (a * (2 * a2 - b) + c) / 2; 162 | const double r2 = r * r; 163 | const double q3 = q * q * q; 164 | 165 | if (r2 < q3) { 166 | const double qsqrt = std::sqrt(q); 167 | const double t = std::min(std::max(r / (q * qsqrt), -1.0), 1.0); 168 | q = -2 * qsqrt; 169 | 170 | const double theta = std::acos(t) / 3; 171 | const double ux = std::cos(theta) * q; 172 | const double uyi = std::sin(theta) * q; 173 | x[0] = ux - a; 174 | x[1] = ux * cos120 - uyi * sin120 - a; 175 | x[2] = ux * cos120 + uyi * sin120 - a; 176 | return 3; 177 | 178 | } else { 179 | double A = -std::cbrt(std::abs(r) + std::sqrt(r2 - q3)); 180 | if (r < 0.0) { 181 | A = -A; 182 | } 183 | const double B = (0.0 == A ? 0.0 : q / A); 184 | 185 | x[0] = (A + B) - a; 186 | x[1] = -(A + B) / 2 - a; 187 | x[2] = std::sqrt(3) * (A - B) / 2; 188 | if (std::abs(x[2]) < DBL_EPSILON) { 189 | x[2] = x[1]; 190 | return 2; 191 | } 192 | 193 | return 1; 194 | } 195 | } 196 | 197 | //! Calculate all roots of the monic quartic equation: x^4 + a*x^3 + b*x^2 + c*x + d = 0 198 | inline PositiveSet solve_quart_monic(double a, double b, double c, double d) { 199 | PositiveSet roots; 200 | 201 | if (std::abs(d) < DBL_EPSILON) { 202 | if (std::abs(c) < DBL_EPSILON) { 203 | roots.insert(0.0); 204 | 205 | const double D = a * a - 4 * b; 206 | if (std::abs(D) < DBL_EPSILON) { 207 | roots.insert(-a / 2); 208 | } else if (D > 0.0) { 209 | const double sqrtD = std::sqrt(D); 210 | roots.insert((-a - sqrtD) / 2); 211 | roots.insert((-a + sqrtD) / 2); 212 | } 213 | return roots; 214 | } 215 | 216 | if (std::abs(a) < DBL_EPSILON && std::abs(b) < DBL_EPSILON) { 217 | roots.insert(0.0); 218 | roots.insert(-std::cbrt(c)); 219 | return roots; 220 | } 221 | } 222 | 223 | const double a3 = -b; 224 | const double b3 = a * c - 4 * d; 225 | const double c3 = -a * a * d - c * c + 4 * b * d; 226 | 227 | std::array x3; 228 | const int number_zeroes = solve_resolvent(x3, a3, b3, c3); 229 | 230 | double y = x3[0]; 231 | // Choosing Y with maximal absolute value. 232 | if (number_zeroes != 1) { 233 | if (std::abs(x3[1]) > std::abs(y)) { 234 | y = x3[1]; 235 | } 236 | if (std::abs(x3[2]) > std::abs(y)) { 237 | y = x3[2]; 238 | } 239 | } 240 | 241 | double q1, q2, p1, p2; 242 | 243 | double D = y * y - 4 * d; 244 | if (std::abs(D) < DBL_EPSILON) { 245 | q1 = q2 = y / 2; 246 | D = a * a - 4 * (b - y); 247 | if (std::abs(D) < DBL_EPSILON) { 248 | p1 = p2 = a / 2; 249 | } else { 250 | const double sqrtD = std::sqrt(D); 251 | p1 = (a + sqrtD) / 2; 252 | p2 = (a - sqrtD) / 2; 253 | } 254 | } else { 255 | const double sqrtD = std::sqrt(D); 256 | q1 = (y + sqrtD) / 2; 257 | q2 = (y - sqrtD) / 2; 258 | p1 = (a * q1 - c) / (q1 - q2); 259 | p2 = (c - a * q2) / (q1 - q2); 260 | } 261 | 262 | constexpr double eps {16 * DBL_EPSILON}; 263 | 264 | D = p1 * p1 - 4 * q1; 265 | if (std::abs(D) < eps) { 266 | roots.insert(-p1 / 2); 267 | } else if (D > 0.0) { 268 | const double sqrtD = std::sqrt(D); 269 | roots.insert((-p1 - sqrtD) / 2); 270 | roots.insert((-p1 + sqrtD) / 2); 271 | } 272 | 273 | D = p2 * p2 - 4 * q2; 274 | if (std::abs(D) < eps) { 275 | roots.insert(-p2 / 2); 276 | } else if (D > 0.0) { 277 | const double sqrtD = std::sqrt(D); 278 | roots.insert((-p2 - sqrtD) / 2); 279 | roots.insert((-p2 + sqrtD) / 2); 280 | } 281 | 282 | return roots; 283 | } 284 | 285 | //! Calculate the quartic equation: x^4 + b*x^3 + c*x^2 + d*x + e = 0 286 | inline PositiveSet solve_quart_monic(const std::array& polynom) { 287 | return solve_quart_monic(polynom[0], polynom[1], polynom[2], polynom[3]); 288 | } 289 | 290 | 291 | //! Evaluate a polynomial of order N at x 292 | template 293 | inline double poly_eval(const std::array& p, double x) { 294 | double retVal = 0.0; 295 | if constexpr (N == 0) { 296 | return retVal; 297 | } 298 | 299 | if (std::abs(x) < DBL_EPSILON) { 300 | retVal = p[N - 1]; 301 | } else if (x == 1.0) { 302 | for (int i = N - 1; i >= 0; i--) { 303 | retVal += p[i]; 304 | } 305 | } else { 306 | double xn = 1.0; 307 | 308 | for (int i = N - 1; i >= 0; i--) { 309 | retVal += p[i] * xn; 310 | xn *= x; 311 | } 312 | } 313 | 314 | return retVal; 315 | } 316 | 317 | // Calculate the derivative poly coefficients of a given poly 318 | template 319 | inline std::array poly_derivative(const std::array& coeffs) { 320 | std::array deriv; 321 | for (size_t i = 0; i < N - 1; ++i) { 322 | deriv[i] = (N - 1 - i) * coeffs[i]; 323 | } 324 | return deriv; 325 | } 326 | 327 | template 328 | inline std::array poly_monic_derivative(const std::array& monic_coeffs) { 329 | std::array deriv; 330 | deriv[0] = 1.0; 331 | for (size_t i = 1; i < N - 1; ++i) { 332 | deriv[i] = (N - 1 - i) * monic_coeffs[i] / (N - 1); 333 | } 334 | return deriv; 335 | } 336 | 337 | // Safe Newton Method 338 | constexpr double tolerance {1e-14}; 339 | 340 | // Calculate a single zero of polynom p(x) inside [lbound, ubound] 341 | // Requirements: p(lbound)*p(ubound) < 0, lbound < ubound 342 | template 343 | inline double shrink_interval(const std::array& p, double l, double h) { 344 | const double fl = poly_eval(p, l); 345 | const double fh = poly_eval(p, h); 346 | if (fl == 0.0) { 347 | return l; 348 | } 349 | if (fh == 0.0) { 350 | return h; 351 | } 352 | if (fl > 0.0) { 353 | std::swap(l, h); 354 | } 355 | 356 | double rts = (l + h) / 2; 357 | double dxold = std::abs(h - l); 358 | double dx = dxold; 359 | const auto deriv = poly_derivative(p); 360 | double f = poly_eval(p, rts); 361 | double df = poly_eval(deriv, rts); 362 | double temp; 363 | for (size_t j = 0; j < maxIts; j++) { 364 | if ((((rts - h) * df - f) * ((rts - l) * df - f) > 0.0) || (std::abs(2 * f) > std::abs(dxold * df))) { 365 | dxold = dx; 366 | dx = (h - l) / 2; 367 | rts = l + dx; 368 | if (l == rts) { 369 | break; 370 | } 371 | } else { 372 | dxold = dx; 373 | dx = f / df; 374 | temp = rts; 375 | rts -= dx; 376 | if (temp == rts) { 377 | break; 378 | } 379 | } 380 | 381 | if (std::abs(dx) < tolerance) { 382 | break; 383 | } 384 | 385 | f = poly_eval(p, rts); 386 | df = poly_eval(deriv, rts); 387 | if (f < 0.0) { 388 | l = rts; 389 | } else { 390 | h = rts; 391 | } 392 | } 393 | 394 | return rts; 395 | } 396 | 397 | } // namespace roots 398 | 399 | } // namespace ruckig 400 | -------------------------------------------------------------------------------- /include/ruckig/ruckig.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | 20 | namespace ruckig { 21 | 22 | //! Main interface for the Ruckig algorithm 23 | template class CustomVector = StandardVector, bool throw_error = false> 24 | class Ruckig { 25 | //! Current input, only for comparison for recalculation 26 | InputParameter current_input; 27 | 28 | //! Flag that indicates if the current_input was properly initialized 29 | bool current_input_initialized {false}; 30 | 31 | public: 32 | //! Calculator for new trajectories 33 | Calculator calculator; 34 | 35 | //! Max number of intermediate waypoints 36 | const size_t max_number_of_waypoints; 37 | 38 | //! Degrees of freedom 39 | const size_t degrees_of_freedom; 40 | 41 | //! Time step between updates (cycle time) in [s] 42 | double delta_time {0.0}; 43 | 44 | template= 1), int>::type = 0> 45 | explicit Ruckig(): 46 | max_number_of_waypoints(0), 47 | degrees_of_freedom(DOFs), 48 | delta_time(-1.0) 49 | { 50 | } 51 | 52 | template= 1), int>::type = 0> 53 | explicit Ruckig(double delta_time): 54 | max_number_of_waypoints(0), 55 | degrees_of_freedom(DOFs), 56 | delta_time(delta_time) 57 | { 58 | } 59 | 60 | #if defined WITH_CLOUD_CLIENT 61 | template= 1), int>::type = 0> 62 | explicit Ruckig(double delta_time, size_t max_number_of_waypoints): 63 | current_input(InputParameter(max_number_of_waypoints)), 64 | calculator(Calculator(max_number_of_waypoints)), 65 | max_number_of_waypoints(max_number_of_waypoints), 66 | degrees_of_freedom(DOFs), 67 | delta_time(delta_time) 68 | { 69 | } 70 | #endif 71 | 72 | template::type = 0> 73 | explicit Ruckig(size_t dofs): 74 | current_input(InputParameter(dofs)), 75 | calculator(Calculator(dofs)), 76 | max_number_of_waypoints(0), 77 | degrees_of_freedom(dofs), 78 | delta_time(-1.0) 79 | { 80 | } 81 | 82 | template::type = 0> 83 | explicit Ruckig(size_t dofs, double delta_time): 84 | current_input(InputParameter(dofs)), 85 | calculator(Calculator(dofs)), 86 | max_number_of_waypoints(0), 87 | degrees_of_freedom(dofs), 88 | delta_time(delta_time) 89 | { 90 | } 91 | 92 | #if defined WITH_CLOUD_CLIENT 93 | template::type = 0> 94 | explicit Ruckig(size_t dofs, double delta_time, size_t max_number_of_waypoints): 95 | current_input(InputParameter(dofs, max_number_of_waypoints)), 96 | calculator(Calculator(dofs, max_number_of_waypoints)), 97 | max_number_of_waypoints(max_number_of_waypoints), 98 | degrees_of_freedom(dofs), 99 | delta_time(delta_time) 100 | { 101 | } 102 | #endif 103 | 104 | //! Reset the instance (e.g. to force a new calculation in the next update) 105 | void reset() { 106 | current_input_initialized = false; 107 | } 108 | 109 | //! Filter intermediate positions based on a threshold distance for each DoF 110 | template using Vector = CustomVector; 111 | std::vector> filter_intermediate_positions(const InputParameter& input, const Vector& threshold_distance) const { 112 | if (input.intermediate_positions.empty()) { 113 | return input.intermediate_positions; 114 | } 115 | 116 | const size_t n_waypoints = input.intermediate_positions.size(); 117 | std::vector is_active; 118 | is_active.resize(n_waypoints); 119 | for (size_t i = 0; i < n_waypoints; ++i) { 120 | is_active[i] = true; 121 | } 122 | 123 | size_t start = 0; 124 | size_t end = start + 2; 125 | for (; end < n_waypoints + 2; ++end) { 126 | const auto pos_start = (start == 0) ? input.current_position : input.intermediate_positions[start - 1]; 127 | const auto pos_end = (end == n_waypoints + 1) ? input.target_position : input.intermediate_positions[end - 1]; 128 | 129 | // Check for all intermediate positions 130 | bool are_all_below {true}; 131 | for (size_t current = start + 1; current < end; ++current) { 132 | const auto pos_current = input.intermediate_positions[current - 1]; 133 | 134 | // Is there a point t on the line that holds the threshold? 135 | double t_start_max = 0.0; 136 | double t_end_min = 1.0; 137 | for (size_t dof = 0; dof < degrees_of_freedom; ++dof) { 138 | const double h0 = (pos_current[dof] - pos_start[dof]) / (pos_end[dof] - pos_start[dof]); 139 | const double t_start = h0 - threshold_distance[dof] / std::abs(pos_end[dof] - pos_start[dof]); 140 | const double t_end = h0 + threshold_distance[dof] / std::abs(pos_end[dof] - pos_start[dof]); 141 | 142 | t_start_max = std::max(t_start, t_start_max); 143 | t_end_min = std::min(t_end, t_end_min); 144 | 145 | if (t_start_max > t_end_min) { 146 | are_all_below = false; 147 | break; 148 | } 149 | } 150 | if (!are_all_below) { 151 | break; 152 | } 153 | } 154 | 155 | is_active[end - 2] = !are_all_below; 156 | if (!are_all_below) { 157 | start = end - 1; 158 | } 159 | } 160 | 161 | std::vector> filtered_positions; 162 | filtered_positions.reserve(n_waypoints); 163 | for (size_t i = 0; i < n_waypoints; ++i) { 164 | if (is_active[i]) { 165 | filtered_positions.push_back(input.intermediate_positions[i]); 166 | } 167 | } 168 | 169 | return filtered_positions; 170 | } 171 | 172 | //! Validate the input as well as the Ruckig instance for trajectory calculation 173 | template 174 | bool validate_input(const InputParameter& input, bool check_current_state_within_limits = false, bool check_target_state_within_limits = true) const { 175 | if (!input.template validate(check_current_state_within_limits, check_target_state_within_limits)) { 176 | return false; 177 | } 178 | 179 | if (!input.intermediate_positions.empty() && input.control_interface == ControlInterface::Position) { 180 | if (input.intermediate_positions.size() > max_number_of_waypoints) { 181 | if constexpr (throw_validation_error) { 182 | throw RuckigError("The number of intermediate positions " + std::to_string(input.intermediate_positions.size()) + " exceeds the maximum number of waypoints " + std::to_string(max_number_of_waypoints) + "."); 183 | } 184 | return false; 185 | } 186 | } 187 | 188 | if (delta_time <= 0.0 && input.duration_discretization != DurationDiscretization::Continuous) { 189 | if constexpr (throw_validation_error) { 190 | throw RuckigError("delta time (control rate) parameter " + std::to_string(delta_time) + " should be larger than zero."); 191 | } 192 | return false; 193 | } 194 | 195 | return true; 196 | } 197 | 198 | //! Calculate a new trajectory for the given input 199 | Result calculate(const InputParameter& input, Trajectory& trajectory) { 200 | bool was_interrupted {false}; 201 | return calculate(input, trajectory, was_interrupted); 202 | } 203 | 204 | //! Calculate a new trajectory for the given input and check for interruption 205 | Result calculate(const InputParameter& input, Trajectory& trajectory, bool& was_interrupted) { 206 | if (!validate_input(input, false, true)) { 207 | return Result::ErrorInvalidInput; 208 | } 209 | 210 | return calculator.template calculate(input, trajectory, delta_time, was_interrupted); 211 | } 212 | 213 | //! Get the next output state (with step delta_time) along the calculated trajectory for the given input 214 | Result update(const InputParameter& input, OutputParameter& output) { 215 | const auto start = std::chrono::steady_clock::now(); 216 | 217 | if constexpr (DOFs == 0 && throw_error) { 218 | if (degrees_of_freedom != input.degrees_of_freedom || degrees_of_freedom != output.degrees_of_freedom) { 219 | throw RuckigError("mismatch in degrees of freedom (vector size)."); 220 | } 221 | } 222 | 223 | output.new_calculation = false; 224 | 225 | Result result {Result::Working}; 226 | if (!current_input_initialized || input != current_input) { 227 | result = calculate(input, output.trajectory, output.was_calculation_interrupted); 228 | if (result != Result::Working && result != Result::ErrorPositionalLimits) { 229 | return result; 230 | } 231 | 232 | current_input = input; 233 | current_input_initialized = true; 234 | output.time = 0.0; 235 | output.new_calculation = true; 236 | } 237 | 238 | const size_t old_section = output.new_section; 239 | output.time += delta_time; 240 | output.trajectory.at_time(output.time, output.new_position, output.new_velocity, output.new_acceleration, output.new_jerk, output.new_section); 241 | output.did_section_change = (output.new_section > old_section); // Report only forward section changes 242 | 243 | const auto stop = std::chrono::steady_clock::now(); 244 | output.calculation_duration = std::chrono::duration_cast(stop - start).count() / 1000.0; 245 | 246 | output.pass_to_input(current_input); 247 | 248 | if (output.time > output.trajectory.get_duration()) { 249 | return Result::Finished; 250 | } 251 | 252 | return result; 253 | } 254 | }; 255 | 256 | 257 | template class CustomVector = StandardVector> 258 | using RuckigThrow = Ruckig; 259 | 260 | 261 | } // namespace ruckig 262 | -------------------------------------------------------------------------------- /include/ruckig/trajectory.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | 12 | namespace ruckig { 13 | 14 | template class> class TargetCalculator; 15 | template class> class WaypointsCalculator; 16 | 17 | 18 | //! The trajectory generated by the Ruckig algorithm. 19 | template class CustomVector = StandardVector> 20 | class Trajectory { 21 | #if defined WITH_CLOUD_CLIENT 22 | template using Container = std::vector; 23 | #else 24 | template using Container = std::array; 25 | #endif 26 | 27 | template using Vector = CustomVector; 28 | 29 | friend class TargetCalculator; 30 | friend class WaypointsCalculator; 31 | 32 | Container> profiles; 33 | 34 | double duration {0.0}; 35 | Container cumulative_times; 36 | 37 | Vector independent_min_durations; 38 | Vector position_extrema; 39 | 40 | size_t continue_calculation_counter {0}; 41 | 42 | #if defined WITH_CLOUD_CLIENT 43 | template= 1), int>::type = 0> 44 | void resize(size_t max_number_of_waypoints) { 45 | profiles.resize(max_number_of_waypoints + 1); 46 | cumulative_times.resize(max_number_of_waypoints + 1); 47 | } 48 | 49 | template::type = 0> 50 | void resize(size_t max_number_of_waypoints) { 51 | resize<1>(max_number_of_waypoints); // Also call resize method above 52 | 53 | for (auto& p: profiles) { 54 | p.resize(degrees_of_freedom); 55 | } 56 | } 57 | #endif 58 | 59 | //! Calculates the base values to then integrate from 60 | template 61 | void state_to_integrate_from(double time, size_t& new_section, Func&& set_integrate) const { 62 | if (time >= duration) { 63 | // Keep constant acceleration 64 | new_section = profiles.size(); 65 | const auto& profiles_dof = profiles.back(); 66 | for (size_t dof = 0; dof < degrees_of_freedom; ++dof) { 67 | const double t_pre = (profiles.size() > 1) ? cumulative_times[cumulative_times.size() - 2] : profiles_dof[dof].brake.duration; 68 | const double t_diff = time - (t_pre + profiles_dof[dof].t_sum.back()); 69 | set_integrate(dof, t_diff, profiles_dof[dof].p.back(), profiles_dof[dof].v.back(), profiles_dof[dof].a.back(), 0.0); 70 | } 71 | return; 72 | } 73 | 74 | const auto new_section_ptr = std::upper_bound(cumulative_times.begin(), cumulative_times.end(), time); 75 | new_section = std::distance(cumulative_times.begin(), new_section_ptr); 76 | double t_diff = time; 77 | if (new_section > 0) { 78 | t_diff -= cumulative_times[new_section - 1]; 79 | } 80 | 81 | for (size_t dof = 0; dof < degrees_of_freedom; ++dof) { 82 | const Profile& p = profiles[new_section][dof]; 83 | double t_diff_dof = t_diff; 84 | 85 | // Brake pre-trajectory 86 | if (new_section == 0 && p.brake.duration > 0) { 87 | if (t_diff_dof < p.brake.duration) { 88 | const size_t index = (t_diff_dof < p.brake.t[0]) ? 0 : 1; 89 | if (index > 0) { 90 | t_diff_dof -= p.brake.t[index - 1]; 91 | } 92 | 93 | set_integrate(dof, t_diff_dof, p.brake.p[index], p.brake.v[index], p.brake.a[index], p.brake.j[index]); 94 | continue; 95 | } else { 96 | t_diff_dof -= p.brake.duration; 97 | } 98 | } 99 | 100 | // Accel post-trajectory 101 | // if (new_section == profiles.size() - 1 && p.accel.duration > 0) { 102 | // if (t_diff_dof > p.t_sum.back()) { 103 | // const size_t index = (t_diff_dof < p.accel.t[1]) ? 1 : 0; 104 | // if (index > 0) { 105 | // t_diff_dof -= p.accel.t[index - 1]; 106 | // } 107 | 108 | // t_diff_dof -= p.t_sum.back(); 109 | 110 | // if (t_diff_dof < p.accel.t[1]) { 111 | // set_integrate(dof, t_diff_dof, p.p.back(), p.v.back(), p.a.back(), p.accel.j[1]); 112 | // continue; 113 | // } 114 | 115 | // t_diff_dof -= p.accel.t[1]; 116 | 117 | // const size_t index = 1; 118 | // set_integrate(dof, t_diff_dof, p.accel.p[index], p.accel.v[index], p.accel.a[index], p.accel.j[index]); 119 | // continue; 120 | // } 121 | // } 122 | 123 | // Non-time synchronization 124 | if (t_diff_dof >= p.t_sum.back()) { 125 | // Keep constant acceleration 126 | set_integrate(dof, t_diff_dof - p.t_sum.back(), p.p.back(), p.v.back(), p.a.back(), 0.0); 127 | continue; 128 | } 129 | 130 | const auto index_dof_ptr = std::upper_bound(p.t_sum.begin(), p.t_sum.end(), t_diff_dof); 131 | const size_t index_dof = std::distance(p.t_sum.begin(), index_dof_ptr); 132 | 133 | if (index_dof > 0) { 134 | t_diff_dof -= p.t_sum[index_dof - 1]; 135 | } 136 | 137 | set_integrate(dof, t_diff_dof, p.p[index_dof], p.v[index_dof], p.a[index_dof], p.j[index_dof]); 138 | } 139 | } 140 | 141 | public: 142 | size_t degrees_of_freedom; 143 | 144 | template= 1), int>::type = 0> 145 | Trajectory(): degrees_of_freedom(DOFs) { 146 | #if defined WITH_CLOUD_CLIENT 147 | resize(0); 148 | #endif 149 | } 150 | 151 | template::type = 0> 152 | Trajectory(size_t dofs): degrees_of_freedom(dofs) { 153 | #if defined WITH_CLOUD_CLIENT 154 | resize(0); 155 | #endif 156 | 157 | profiles[0].resize(dofs); 158 | independent_min_durations.resize(dofs); 159 | position_extrema.resize(dofs); 160 | } 161 | 162 | #if defined WITH_CLOUD_CLIENT 163 | template= 1), int>::type = 0> 164 | Trajectory(size_t max_number_of_waypoints): degrees_of_freedom(DOFs) { 165 | resize(max_number_of_waypoints); 166 | } 167 | 168 | template::type = 0> 169 | Trajectory(size_t dofs, size_t max_number_of_waypoints): degrees_of_freedom(dofs) { 170 | resize(max_number_of_waypoints); 171 | 172 | independent_min_durations.resize(dofs); 173 | position_extrema.resize(dofs); 174 | } 175 | #endif 176 | 177 | //! Get the kinematic state, the jerk, and the section at a given time 178 | void at_time(double time, Vector& new_position, Vector& new_velocity, Vector& new_acceleration, Vector& new_jerk, size_t& new_section) const { 179 | if constexpr (DOFs == 0) { 180 | if (degrees_of_freedom != new_position.size() || degrees_of_freedom != new_velocity.size() || degrees_of_freedom != new_acceleration.size() || degrees_of_freedom != new_jerk.size()) { 181 | throw RuckigError("mismatch in degrees of freedom (vector size)."); 182 | } 183 | } 184 | 185 | state_to_integrate_from(time, new_section, [&](size_t dof, double t, double p, double v, double a, double j) { 186 | std::tie(new_position[dof], new_velocity[dof], new_acceleration[dof]) = integrate(t, p, v, a, j); 187 | new_jerk[dof] = j; 188 | }); 189 | } 190 | 191 | //! Get the kinematic state at a given time 192 | //! The Python wrapper takes `time` as an argument, and returns `new_position`, `new_velocity`, and `new_acceleration` instead. 193 | void at_time(double time, Vector& new_position, Vector& new_velocity, Vector& new_acceleration) const { 194 | if constexpr (DOFs == 0) { 195 | if (degrees_of_freedom != new_position.size() || degrees_of_freedom != new_velocity.size() || degrees_of_freedom != new_acceleration.size()) { 196 | throw RuckigError("mismatch in degrees of freedom (vector size)."); 197 | } 198 | } 199 | 200 | size_t new_section; 201 | state_to_integrate_from(time, new_section, [&](size_t dof, double t, double p, double v, double a, double j) { 202 | std::tie(new_position[dof], new_velocity[dof], new_acceleration[dof]) = integrate(t, p, v, a, j); 203 | }); 204 | } 205 | 206 | //! Get the position at a given time 207 | void at_time(double time, Vector& new_position) const { 208 | if constexpr (DOFs == 0) { 209 | if (degrees_of_freedom != new_position.size()) { 210 | throw RuckigError("mismatch in degrees of freedom (vector size)."); 211 | } 212 | } 213 | 214 | size_t new_section; 215 | state_to_integrate_from(time, new_section, [&](size_t dof, double t, double p, double v, double a, double j) { 216 | std::tie(new_position[dof], std::ignore, std::ignore) = integrate(t, p, v, a, j); 217 | }); 218 | } 219 | 220 | //! Get the kinematic state, the jerk, and the section at a given time without vectors for a single DoF 221 | template::type = 0> 222 | void at_time(double time, double& new_position, double& new_velocity, double& new_acceleration, double& new_jerk, size_t& new_section) const { 223 | state_to_integrate_from(time, new_section, [&](size_t, double t, double p, double v, double a, double j) { 224 | std::tie(new_position, new_velocity, new_acceleration) = integrate(t, p, v, a, j); 225 | new_jerk = j; 226 | }); 227 | } 228 | 229 | //! Get the kinematic state at a given time without vectors for a single DoF 230 | template::type = 0> 231 | void at_time(double time, double& new_position, double& new_velocity, double& new_acceleration) const { 232 | size_t new_section; 233 | state_to_integrate_from(time, new_section, [&](size_t, double t, double p, double v, double a, double j) { 234 | std::tie(new_position, new_velocity, new_acceleration) = integrate(t, p, v, a, j); 235 | }); 236 | } 237 | 238 | //! Get the position at a given time without vectors for a single DoF 239 | template::type = 0> 240 | void at_time(double time, double& new_position) const { 241 | size_t new_section; 242 | state_to_integrate_from(time, new_section, [&](size_t, double t, double p, double v, double a, double j) { 243 | std::tie(new_position, std::ignore, std::ignore) = integrate(t, p, v, a, j); 244 | }); 245 | } 246 | 247 | 248 | //! Get the underlying profiles of the trajectory (only in the Ruckig Community Version) 249 | Container> get_profiles() const { 250 | return profiles; 251 | } 252 | 253 | //! Get the duration of the (synchronized) trajectory 254 | double get_duration() const { 255 | return duration; 256 | } 257 | 258 | //! Get the durations when the intermediate waypoints are reached 259 | Container get_intermediate_durations() const { 260 | return cumulative_times; 261 | } 262 | 263 | //! Get the minimum duration of each independent DoF 264 | Vector get_independent_min_durations() const { 265 | return independent_min_durations; 266 | } 267 | 268 | //! Get the min/max values of the position for each DoF 269 | Vector get_position_extrema() { 270 | for (size_t dof = 0; dof < degrees_of_freedom; ++dof) { 271 | position_extrema[dof] = profiles[0][dof].get_position_extrema(); 272 | } 273 | 274 | for (size_t i = 1; i < profiles.size(); ++i) { 275 | for (size_t dof = 0; dof < degrees_of_freedom; ++dof) { 276 | auto section_position_extrema = profiles[i][dof].get_position_extrema(); 277 | if (section_position_extrema.max > position_extrema[dof].max) { 278 | position_extrema[dof].max = section_position_extrema.max; 279 | position_extrema[dof].t_max = section_position_extrema.t_max; 280 | } 281 | if (section_position_extrema.min < position_extrema[dof].min) { 282 | position_extrema[dof].min = section_position_extrema.min; 283 | position_extrema[dof].t_min = section_position_extrema.t_min; 284 | } 285 | } 286 | } 287 | 288 | return position_extrema; 289 | } 290 | 291 | //! Get the time that this trajectory passes a specific position of a given DoF the first time 292 | std::optional get_first_time_at_position(size_t dof, double position, double time_after=0.0) const { 293 | if (dof >= degrees_of_freedom) { 294 | return std::nullopt; 295 | } 296 | 297 | double time; 298 | for (size_t i = 0; i < profiles.size(); ++i) { 299 | if (profiles[i][dof].get_first_state_at_position(position, time, time_after)) { 300 | const double section_time = (i > 0) ? cumulative_times[i-1] : 0.0; 301 | return section_time + time; 302 | } 303 | } 304 | return std::nullopt; 305 | } 306 | }; 307 | 308 | } // namespace ruckig 309 | -------------------------------------------------------------------------------- /include/ruckig/utils.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | 12 | namespace ruckig { 13 | 14 | //! Constant for indicating a dynamic (run-time settable) number of DoFs 15 | constexpr static size_t DynamicDOFs {0}; 16 | 17 | 18 | //! Vector data type based on the C++ standard library 19 | template using StandardVector = typename std::conditional= 1, std::array, std::vector>::type; 20 | template using StandardSizeVector = typename std::conditional= 1, std::array, std::vector>::type; 21 | 22 | 23 | //! Vector data type based on the Eigen matrix type. Eigen needs to be included seperately 24 | #ifdef EIGEN_VERSION_AT_LEAST 25 | #if EIGEN_VERSION_AT_LEAST(3,3,7) 26 | template using EigenVector = typename std::conditional= 1, Eigen::Matrix, Eigen::Matrix>::type; 27 | #endif 28 | #endif 29 | 30 | 31 | template 32 | inline std::string join(const Vector& array, bool high_precision = false) { 33 | std::ostringstream ss; 34 | for (size_t i = 0; i < array.size(); ++i) { 35 | if (i) ss << ", "; 36 | if (high_precision) ss << std::setprecision(16); 37 | ss << array[i]; 38 | } 39 | return ss.str(); 40 | } 41 | 42 | 43 | //! Integrate with constant jerk for duration t. Returns new position, new velocity, and new acceleration. 44 | inline std::tuple integrate(double t, double p0, double v0, double a0, double j) { 45 | return std::make_tuple( 46 | p0 + t * (v0 + t * (a0 / 2 + t * j / 6)), 47 | v0 + t * (a0 + t * j / 2), 48 | a0 + t * j 49 | ); 50 | } 51 | 52 | } // namespace ruckig 53 | -------------------------------------------------------------------------------- /include/ruckig/velocity.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | 7 | namespace ruckig { 8 | 9 | //! Mathematical equations for Step 1 in third-order velocity interface: Extremal profiles 10 | class VelocityThirdOrderStep1 { 11 | using ReachedLimits = Profile::ReachedLimits; 12 | using ControlSigns = Profile::ControlSigns; 13 | 14 | const double a0, af; 15 | const double _aMax, _aMin, _jMax; 16 | 17 | // Pre-calculated expressions 18 | double vd; 19 | 20 | // Max 3 valid profiles 21 | using ProfileIter = std::array::iterator; 22 | std::array valid_profiles; 23 | 24 | void time_acc0(ProfileIter& profile, double aMax, double aMin, double jMax, bool return_after_found) const; 25 | void time_none(ProfileIter& profile, double aMax, double aMin, double jMax, bool return_after_found) const; 26 | 27 | // Only for zero-limits case 28 | bool time_all_single_step(Profile* profile, double aMax, double aMin, double jMax) const; 29 | 30 | inline void add_profile(ProfileIter& profile) const { 31 | const auto prev_profile = profile; 32 | ++profile; 33 | profile->set_boundary(*prev_profile); 34 | } 35 | 36 | public: 37 | explicit VelocityThirdOrderStep1(double v0, double a0, double vf, double af, double aMax, double aMin, double jMax); 38 | 39 | bool get_profile(const Profile& input, Block& block); 40 | }; 41 | 42 | 43 | //! Mathematical equations for Step 2 in third-order velocity interface: Time synchronization 44 | class VelocityThirdOrderStep2 { 45 | using ReachedLimits = Profile::ReachedLimits; 46 | using ControlSigns = Profile::ControlSigns; 47 | 48 | const double a0, tf, af; 49 | const double _aMax, _aMin, _jMax; 50 | 51 | // Pre-calculated expressions 52 | double vd, ad; 53 | 54 | bool time_acc0(Profile& profile, double aMax, double aMin, double jMax); 55 | bool time_none(Profile& profile, double aMax, double aMin, double jMax); 56 | 57 | inline bool check_all(Profile& profile, double aMax, double aMin, double jMax) { 58 | return time_acc0(profile, aMax, aMin, jMax) || time_none(profile, aMax, aMin, jMax); 59 | } 60 | 61 | public: 62 | explicit VelocityThirdOrderStep2(double tf, double v0, double a0, double vf, double af, double aMax, double aMin, double jMax); 63 | 64 | bool get_profile(Profile& profile); 65 | }; 66 | 67 | 68 | //! Mathematical equations for Step 1 in second-order velocity interface: Extremal profiles 69 | class VelocitySecondOrderStep1 { 70 | const double _aMax, _aMin; 71 | double vd; // Pre-calculated expressions 72 | 73 | public: 74 | explicit VelocitySecondOrderStep1(double v0, double vf, double aMax, double aMin); 75 | 76 | bool get_profile(const Profile& input, Block& block); 77 | }; 78 | 79 | 80 | //! Mathematical equations for Step 2 in second-order velocity interface: Time synchronization 81 | class VelocitySecondOrderStep2 { 82 | const double tf, _aMax, _aMin; 83 | double vd; // Pre-calculated expressions 84 | 85 | public: 86 | explicit VelocitySecondOrderStep2(double tf, double v0, double vf, double aMax, double aMin); 87 | 88 | bool get_profile(Profile& profile); 89 | }; 90 | 91 | } // namespace ruckig 92 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ruckig 5 | 0.15.3 6 | Instantaneous Motion Generation for Robots and Machines. 7 | Lars Berscheid 8 | Lars Berscheid 9 | MIT 10 | 11 | cmake 12 | 13 | 14 | cmake 15 | 16 | 17 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [project] 2 | name = "ruckig" 3 | version = "0.15.3" 4 | authors = [ 5 | {name = "Lars Berscheid", email = "lars.berscheid@ruckig.com"}, 6 | ] 7 | readme = "README.md" 8 | description = "Instantaneous Motion Generation for Robots and Machines." 9 | keywords = ["robotics", "trajectory-generation", "real-time", "jerk-constrained", "time-optimal"] 10 | license = {text = "MIT License"} 11 | classifiers = [ 12 | "Development Status :: 5 - Production/Stable", 13 | "Intended Audience :: Science/Research", 14 | "Topic :: Scientific/Engineering", 15 | "License :: OSI Approved :: MIT License", 16 | "Programming Language :: C++", 17 | ] 18 | requires-python = ">=3.8" 19 | 20 | [project.urls] 21 | Homepage = "https://ruckig.com" 22 | Documentation = "https://docs.ruckig.com" 23 | Repository = "https://github.com/pantor/ruckig.git" 24 | Issues = "https://github.com/pantor/ruckig/issues" 25 | 26 | 27 | [build-system] 28 | requires = ["scikit-build-core", "nanobind"] 29 | build-backend = "scikit_build_core.build" 30 | 31 | [tool.scikit-build] 32 | cmake.targets = ["python_ruckig"] 33 | sdist.exclude = [".github"] 34 | 35 | [tool.scikit-build.cmake.define] 36 | BUILD_PYTHON_MODULE = "ON" 37 | BUILD_SHARED_LIBS = "OFF" 38 | CMAKE_POSITION_INDEPENDENT_CODE = "ON" 39 | 40 | 41 | [tool.ruff] 42 | line-length = 160 43 | lint.select = ["A", "COM", "E", "F", "G", "N", "PIE", "PTH", "PYI", "RSE", "RET", "SIM", "TCH", "W", "Q"] 44 | 45 | [tool.ruff.lint.flake8-quotes] 46 | inline-quotes = "single" 47 | multiline-quotes = "single" 48 | -------------------------------------------------------------------------------- /src/ruckig/brake.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | 4 | namespace ruckig { 5 | 6 | inline double v_at_t(double v0, double a0, double j, double t) { 7 | return v0 + t * (a0 + j * t / 2); 8 | } 9 | 10 | inline double v_at_a_zero(double v0, double a0, double j) { 11 | return v0 + (a0 * a0)/(2 * j); 12 | } 13 | 14 | void BrakeProfile::acceleration_brake(double v0, double a0, double vMax, double vMin, double aMax, double aMin, double jMax) { 15 | j[0] = -jMax; 16 | 17 | const double t_to_a_max = (a0 - aMax) / jMax; 18 | const double t_to_a_zero = a0 / jMax; 19 | 20 | const double v_at_a_max = v_at_t(v0, a0, -jMax, t_to_a_max); 21 | const double v_at_a_zero = v_at_t(v0, a0, -jMax, t_to_a_zero); 22 | 23 | if ((v_at_a_zero > vMax && jMax > 0) || (v_at_a_zero < vMax && jMax < 0)) { 24 | velocity_brake(v0, a0, vMax, vMin, aMax, aMin, jMax); 25 | 26 | } else if ((v_at_a_max < vMin && jMax > 0) || (v_at_a_max > vMin && jMax < 0)) { 27 | const double t_to_v_min = -(v_at_a_max - vMin)/aMax; 28 | const double t_to_v_max = -aMax/(2*jMax) - (v_at_a_max - vMax)/aMax; 29 | 30 | t[0] = t_to_a_max + eps; 31 | t[1] = std::max(std::min(t_to_v_min, t_to_v_max - eps), 0.0); 32 | 33 | } else { 34 | t[0] = t_to_a_max + eps; 35 | } 36 | } 37 | 38 | void BrakeProfile::velocity_brake(double v0, double a0, double vMax, double vMin, double, double aMin, double jMax) { 39 | j[0] = -jMax; 40 | const double t_to_a_min = (a0 - aMin)/jMax; 41 | const double t_to_v_max = a0/jMax + std::sqrt(a0*a0 + 2 * jMax * (v0 - vMax)) / std::abs(jMax); 42 | const double t_to_v_min = a0/jMax + std::sqrt(a0*a0 / 2 + jMax * (v0 - vMin)) / std::abs(jMax); 43 | const double t_min_to_v_max = std::min(t_to_v_max, t_to_v_min); 44 | 45 | if (t_to_a_min < t_min_to_v_max) { 46 | const double v_at_a_min = v_at_t(v0, a0, -jMax, t_to_a_min); 47 | const double t_to_v_max_with_constant = -(v_at_a_min - vMax)/aMin; 48 | const double t_to_v_min_with_constant = aMin/(2*jMax) - (v_at_a_min - vMin)/aMin; 49 | 50 | t[0] = std::max(t_to_a_min - eps, 0.0); 51 | t[1] = std::max(std::min(t_to_v_max_with_constant, t_to_v_min_with_constant), 0.0); 52 | 53 | } else { 54 | t[0] = std::max(t_min_to_v_max - eps, 0.0); 55 | } 56 | } 57 | 58 | void BrakeProfile::get_position_brake_trajectory(double v0, double a0, double vMax, double vMin, double aMax, double aMin, double jMax) { 59 | t[0] = 0.0; 60 | t[1] = 0.0; 61 | j[0] = 0.0; 62 | j[1] = 0.0; 63 | 64 | if (jMax == 0.0 || aMax == 0.0 || aMin == 0.0) { 65 | return; // Ignore braking for zero-limits 66 | } 67 | 68 | if (a0 > aMax) { 69 | acceleration_brake(v0, a0, vMax, vMin, aMax, aMin, jMax); 70 | 71 | } else if (a0 < aMin) { 72 | acceleration_brake(v0, a0, vMin, vMax, aMin, aMax, -jMax); 73 | 74 | } else if ((v0 > vMax && v_at_a_zero(v0, a0, -jMax) > vMin) || (a0 > 0 && v_at_a_zero(v0, a0, jMax) > vMax)) { 75 | velocity_brake(v0, a0, vMax, vMin, aMax, aMin, jMax); 76 | 77 | } else if ((v0 < vMin && v_at_a_zero(v0, a0, jMax) < vMax) || (a0 < 0 && v_at_a_zero(v0, a0, -jMax) < vMin)) { 78 | velocity_brake(v0, a0, vMin, vMax, aMin, aMax, -jMax); 79 | } 80 | } 81 | 82 | void BrakeProfile::get_second_order_position_brake_trajectory(double v0, double vMax, double vMin, double aMax, double aMin) { 83 | t[0] = 0.0; 84 | t[1] = 0.0; 85 | j[0] = 0.0; 86 | j[1] = 0.0; 87 | a[0] = 0.0; 88 | a[1] = 0.0; 89 | 90 | if (aMax == 0.0 || aMin == 0.0) { 91 | return; // Ignore braking for zero-limits 92 | } 93 | 94 | if (v0 > vMax) { 95 | a[0] = aMin; 96 | t[0] = (vMax - v0)/aMin + eps; 97 | 98 | } else if (v0 < vMin) { 99 | a[0] = aMax; 100 | t[0] = (vMin - v0)/aMax + eps; 101 | } 102 | } 103 | 104 | void BrakeProfile::get_velocity_brake_trajectory(double a0, double aMax, double aMin, double jMax) { 105 | t[0] = 0.0; 106 | t[1] = 0.0; 107 | j[0] = 0.0; 108 | j[1] = 0.0; 109 | 110 | if (jMax == 0.0) { 111 | return; // Ignore braking for zero-limits 112 | } 113 | 114 | if (a0 > aMax) { 115 | j[0] = -jMax; 116 | t[0] = (a0 - aMax)/jMax + eps; 117 | 118 | } else if (a0 < aMin) { 119 | j[0] = jMax; 120 | t[0] = -(a0 - aMin)/jMax + eps; 121 | } 122 | } 123 | 124 | void BrakeProfile::get_second_order_velocity_brake_trajectory() { 125 | t[0] = 0.0; 126 | t[1] = 0.0; 127 | j[0] = 0.0; 128 | j[1] = 0.0; 129 | } 130 | 131 | } // namespace ruckig 132 | -------------------------------------------------------------------------------- /src/ruckig/cloud_client.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | 6 | namespace ruckig { 7 | 8 | CloudClient::CloudClient() { 9 | cli = std::make_shared("http://api.ruckig.com"); 10 | } 11 | 12 | nlohmann::json CloudClient::post(const nlohmann::json& params, bool throw_error) { 13 | const auto res = cli->Post("/calculate", params.dump(), "application/json"); 14 | if (!res) { 15 | if (throw_error) { 16 | throw RuckigError("could not reach cloud API server"); 17 | } else { 18 | std::cout << "[ruckig] could not reach cloud API server" << std::endl; 19 | return Result::Error; 20 | } 21 | } 22 | if (res->status != 200) { 23 | if (throw_error) { 24 | throw RuckigError("could not reach cloud API server, error code: " + std::to_string(res->status) + " " + res->body); 25 | } else { 26 | std::cout << "[ruckig] could not reach cloud API server, error code: " << res->status << " " << res->body << std::endl; 27 | return Result::Error; 28 | } 29 | } 30 | 31 | return nlohmann::json::parse(res->body); 32 | } 33 | 34 | } -------------------------------------------------------------------------------- /src/ruckig/position_first_step1.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | namespace ruckig { 6 | 7 | PositionFirstOrderStep1::PositionFirstOrderStep1(double p0, double pf, double vMax, double vMin): _vMax(vMax), _vMin(vMin) { 8 | pd = pf - p0; 9 | } 10 | 11 | bool PositionFirstOrderStep1::get_profile(const Profile& input, Block& block) { 12 | auto& p = block.p_min; 13 | p.set_boundary(input); 14 | 15 | const double vf = (pd > 0) ? _vMax : _vMin; 16 | p.t[0] = 0; 17 | p.t[1] = 0; 18 | p.t[2] = 0; 19 | p.t[3] = pd / vf; 20 | p.t[4] = 0; 21 | p.t[5] = 0; 22 | p.t[6] = 0; 23 | 24 | if (p.check_for_first_order(vf)) { 25 | block.t_min = p.t_sum.back() + p.brake.duration + p.accel.duration; 26 | return true; 27 | } 28 | return false; 29 | } 30 | 31 | } // namespace ruckig 32 | -------------------------------------------------------------------------------- /src/ruckig/position_first_step2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | 7 | namespace ruckig { 8 | 9 | PositionFirstOrderStep2::PositionFirstOrderStep2(double tf, double p0, double pf, double vMax, double vMin): tf(tf), _vMax(vMax), _vMin(vMin) { 10 | pd = pf - p0; 11 | } 12 | 13 | bool PositionFirstOrderStep2::get_profile(Profile& profile) { 14 | const double vf = pd / tf; 15 | 16 | profile.t[0] = 0; 17 | profile.t[1] = 0; 18 | profile.t[2] = 0; 19 | profile.t[3] = tf; 20 | profile.t[4] = 0; 21 | profile.t[5] = 0; 22 | profile.t[6] = 0; 23 | 24 | return profile.check_for_first_order_with_timing(tf, vf, _vMax, _vMin); 25 | } 26 | 27 | } // namespace ruckig 28 | -------------------------------------------------------------------------------- /src/ruckig/position_second_step1.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | namespace ruckig { 6 | 7 | PositionSecondOrderStep1::PositionSecondOrderStep1(double p0, double v0, double pf, double vf, double vMax, double vMin, double aMax, double aMin): v0(v0), vf(vf), _vMax(vMax), _vMin(vMin), _aMax(aMax), _aMin(aMin) { 8 | pd = pf - p0; 9 | } 10 | 11 | void PositionSecondOrderStep1::time_acc0(ProfileIter& profile, double vMax, double vMin, double aMax, double aMin, bool) const { 12 | profile->t[0] = (-v0 + vMax)/aMax; 13 | profile->t[1] = (aMin*v0*v0 - aMax*vf*vf)/(2*aMax*aMin*vMax) + vMax*(aMax - aMin)/(2*aMax*aMin) + pd/vMax; 14 | profile->t[2] = (vf - vMax)/aMin; 15 | profile->t[3] = 0; 16 | profile->t[4] = 0; 17 | profile->t[5] = 0; 18 | profile->t[6] = 0; 19 | 20 | if (profile->check_for_second_order(aMax, aMin, vMax, vMin)) { 21 | add_profile(profile); 22 | } 23 | } 24 | 25 | void PositionSecondOrderStep1::time_none(ProfileIter& profile, double vMax, double vMin, double aMax, double aMin, bool return_after_found) const { 26 | double h1 = (aMax*vf*vf - aMin*v0*v0 - 2*aMax*aMin*pd)/(aMax - aMin); 27 | if (h1 >= 0.0) { 28 | h1 = std::sqrt(h1); 29 | 30 | // Solution 1 31 | { 32 | profile->t[0] = -(v0 + h1)/aMax; 33 | profile->t[1] = 0; 34 | profile->t[2] = (vf + h1)/aMin; 35 | profile->t[3] = 0; 36 | profile->t[4] = 0; 37 | profile->t[5] = 0; 38 | profile->t[6] = 0; 39 | 40 | if (profile->check_for_second_order(aMax, aMin, vMax, vMin)) { 41 | add_profile(profile); 42 | if (return_after_found) { 43 | return; 44 | } 45 | } 46 | } 47 | 48 | // Solution 2 49 | { 50 | profile->t[0] = (-v0 + h1)/aMax; 51 | profile->t[1] = 0; 52 | profile->t[2] = (vf - h1)/aMin; 53 | profile->t[3] = 0; 54 | profile->t[4] = 0; 55 | profile->t[5] = 0; 56 | profile->t[6] = 0; 57 | 58 | if (profile->check_for_second_order(aMax, aMin, vMax, vMin)) { 59 | add_profile(profile); 60 | } 61 | } 62 | } 63 | } 64 | 65 | bool PositionSecondOrderStep1::time_all_single_step(Profile* profile, double vMax, double vMin, double aMax, double aMin) const { 66 | if (std::abs(vf - v0) > DBL_EPSILON) { 67 | return false; 68 | } 69 | 70 | profile->t[0] = 0; 71 | profile->t[1] = 0; 72 | profile->t[2] = 0; 73 | profile->t[3] = 0; 74 | profile->t[4] = 0; 75 | profile->t[5] = 0; 76 | profile->t[6] = 0; 77 | 78 | if (std::abs(v0) > DBL_EPSILON) { 79 | profile->t[3] = pd / v0; 80 | if (profile->check_for_second_order(0.0, 0.0, vMax, vMin)) { 81 | return true; 82 | } 83 | 84 | } else if (std::abs(pd) < DBL_EPSILON) { 85 | if (profile->check_for_second_order(0.0, 0.0, vMax, vMin)) { 86 | return true; 87 | } 88 | } 89 | 90 | return false; 91 | } 92 | 93 | 94 | bool PositionSecondOrderStep1::get_profile(const Profile& input, Block& block) { 95 | // Zero-limits special case 96 | if (_vMax == 0.0 && _vMin == 0.0) { 97 | auto& p = block.p_min; 98 | p.set_boundary(input); 99 | 100 | if (time_all_single_step(&p, _vMax, _vMin, _aMax, _aMin)) { 101 | block.t_min = p.t_sum.back() + p.brake.duration + p.accel.duration; 102 | if (std::abs(v0) > DBL_EPSILON) { 103 | block.a = Block::Interval(block.t_min, std::numeric_limits::infinity()); 104 | } 105 | return true; 106 | } 107 | return false; 108 | } 109 | 110 | const ProfileIter start = valid_profiles.begin(); 111 | ProfileIter profile = start; 112 | profile->set_boundary(input); 113 | 114 | if (std::abs(vf) < DBL_EPSILON) { 115 | // There is no blocked interval when vf==0, so return after first found profile 116 | const double vMax = (pd >= 0) ? _vMax : _vMin; 117 | const double vMin = (pd >= 0) ? _vMin : _vMax; 118 | const double aMax = (pd >= 0) ? _aMax : _aMin; 119 | const double aMin = (pd >= 0) ? _aMin : _aMax; 120 | 121 | time_none(profile, vMax, vMin, aMax, aMin, true); 122 | if (profile > start) { goto return_block; } 123 | time_acc0(profile, vMax, vMin, aMax, aMin, true); 124 | if (profile > start) { goto return_block; } 125 | 126 | time_none(profile, vMin, vMax, aMin, aMax, true); 127 | if (profile > start) { goto return_block; } 128 | time_acc0(profile, vMin, vMax, aMin, aMax, true); 129 | 130 | } else { 131 | time_none(profile, _vMax, _vMin, _aMax, _aMin, false); 132 | time_none(profile, _vMin, _vMax, _aMin, _aMax, false); 133 | time_acc0(profile, _vMax, _vMin, _aMax, _aMin, false); 134 | time_acc0(profile, _vMin, _vMax, _aMin, _aMax, false); 135 | } 136 | 137 | return_block: 138 | return Block::calculate_block(block, valid_profiles, std::distance(start, profile)); 139 | } 140 | 141 | } // namespace ruckig 142 | -------------------------------------------------------------------------------- /src/ruckig/position_second_step2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | 7 | namespace ruckig { 8 | 9 | PositionSecondOrderStep2::PositionSecondOrderStep2(double tf, double p0, double v0, double pf, double vf, double vMax, double vMin, double aMax, double aMin): v0(v0), tf(tf), vf(vf), _vMax(vMax), _vMin(vMin), _aMax(aMax), _aMin(aMin) { 10 | pd = pf - p0; 11 | vd = vf - v0; 12 | } 13 | 14 | bool PositionSecondOrderStep2::time_acc0(Profile& profile, double vMax, double vMin, double aMax, double aMin) { 15 | // UD Solution 1/2 16 | { 17 | const double h1 = std::sqrt((2*aMax*(pd - tf*vf) - 2*aMin*(pd - tf*v0) + vd*vd)/(aMax*aMin) + tf*tf); 18 | 19 | profile.t[0] = (aMax*vd - aMax*aMin*(tf - h1))/(aMax*(aMax - aMin)); 20 | profile.t[1] = h1; 21 | profile.t[2] = tf - (profile.t[0] + h1); 22 | profile.t[3] = 0; 23 | profile.t[4] = 0; 24 | profile.t[5] = 0; 25 | profile.t[6] = 0; 26 | 27 | if (profile.check_for_second_order_with_timing(tf, aMax, aMin, vMax, vMin)) { 28 | profile.pf = profile.p.back(); 29 | return true; 30 | } 31 | } 32 | 33 | // UU Solution 34 | { 35 | const double h1 = (-vd + aMax*tf); 36 | 37 | profile.t[0] = -vd*vd/(2*aMax*h1) + (pd - v0*tf)/h1; 38 | profile.t[1] = -vd/aMax + tf; 39 | profile.t[2] = 0; 40 | profile.t[3] = 0; 41 | profile.t[4] = 0; 42 | profile.t[5] = 0; 43 | profile.t[6] = tf - (profile.t[0] + profile.t[1]); 44 | 45 | if (profile.check_for_second_order_with_timing(tf, aMax, aMin, vMax, vMin)) { 46 | profile.pf = profile.p.back(); 47 | return true; 48 | } 49 | } 50 | 51 | // UU Solution - 2 step 52 | { 53 | profile.t[0] = 0; 54 | profile.t[1] = -vd/aMax + tf; 55 | profile.t[2] = 0; 56 | profile.t[3] = 0; 57 | profile.t[4] = 0; 58 | profile.t[5] = 0; 59 | profile.t[6] = vd/aMax; 60 | 61 | if (profile.check_for_second_order_with_timing(tf, aMax, aMin, vMax, vMin)) { 62 | profile.pf = profile.p.back(); 63 | return true; 64 | } 65 | } 66 | 67 | return false; 68 | } 69 | 70 | bool PositionSecondOrderStep2::time_none(Profile& profile, double vMax, double vMin, double aMax, double aMin) { 71 | if (std::abs(v0) < DBL_EPSILON && std::abs(vf) < DBL_EPSILON && std::abs(pd) < DBL_EPSILON) { 72 | profile.t[0] = 0; 73 | profile.t[1] = tf; 74 | profile.t[2] = 0; 75 | profile.t[3] = 0; 76 | profile.t[4] = 0; 77 | profile.t[5] = 0; 78 | profile.t[6] = 0; 79 | 80 | if (profile.check_for_second_order_with_timing(tf, aMax, aMin, vMax, vMin)) { 81 | profile.pf = profile.p.back(); 82 | return true; 83 | } 84 | } 85 | 86 | // UD Solution 1/2 87 | { 88 | const double h1 = 2*(vf*tf - pd); 89 | 90 | profile.t[0] = h1/vd; 91 | profile.t[1] = tf - profile.t[0]; 92 | profile.t[2] = 0; 93 | profile.t[3] = 0; 94 | profile.t[4] = 0; 95 | profile.t[5] = 0; 96 | profile.t[6] = 0; 97 | 98 | const double af = vd*vd/h1; 99 | 100 | if ((aMin - 1e-12 < af) && (af < aMax + 1e-12) && profile.check_for_second_order_with_timing(tf, af, -af, vMax, vMin)) { 101 | profile.pf = profile.p.back(); 102 | return true; 103 | } 104 | } 105 | 106 | return false; 107 | } 108 | 109 | bool PositionSecondOrderStep2::get_profile(Profile& profile) { 110 | // Test all cases to get ones that match 111 | // However we should guess which one is correct and try them first... 112 | if (pd > 0) { 113 | return check_all(profile, _vMax, _vMin, _aMax, _aMin) || check_all(profile, _vMin, _vMax, _aMin, _aMax); 114 | } 115 | 116 | return check_all(profile, _vMin, _vMax, _aMin, _aMax) || check_all(profile, _vMax, _vMin, _aMax, _aMin); 117 | } 118 | 119 | } // namespace ruckig 120 | -------------------------------------------------------------------------------- /src/ruckig/python.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | 14 | namespace nb = nanobind; 15 | using namespace nb::literals; // to bring in the `_a` literal 16 | using namespace ruckig; 17 | 18 | 19 | NB_MODULE(ruckig, m) { 20 | m.doc() = "Instantaneous Motion Generation for Robots and Machines. Real-time and time-optimal trajectory calculation \ 21 | given a target waypoint with position, velocity, and acceleration, starting from any initial state \ 22 | limited by velocity, acceleration, and jerk constraints."; 23 | m.attr("__version__") = "0.15.3"; 24 | 25 | nb::enum_(m, "ControlInterface") 26 | .value("Position", ControlInterface::Position) 27 | .value("Velocity", ControlInterface::Velocity) 28 | .export_values(); 29 | 30 | nb::enum_(m, "Synchronization") 31 | .value("Phase", Synchronization::Phase) 32 | .value("Time", Synchronization::Time) 33 | .value("TimeIfNecessary", Synchronization::TimeIfNecessary) 34 | .value("No", Synchronization::None) 35 | .export_values(); 36 | 37 | nb::enum_(m, "DurationDiscretization") 38 | .value("Continuous", DurationDiscretization::Continuous) 39 | .value("Discrete", DurationDiscretization::Discrete) 40 | .export_values(); 41 | 42 | nb::enum_(m, "Result", nb::is_arithmetic()) 43 | .value("Working", Result::Working) 44 | .value("Finished", Result::Finished) 45 | .value("Error", Result::Error) 46 | .value("ErrorInvalidInput", Result::ErrorInvalidInput) 47 | .value("ErrorPositionalLimits", Result::ErrorPositionalLimits) 48 | .value("ErrorExecutionTimeCalculation", Result::ErrorExecutionTimeCalculation) 49 | .value("ErrorSynchronizationCalculation", Result::ErrorSynchronizationCalculation) 50 | .export_values(); 51 | 52 | nb::exception(m, "RuckigError"); 53 | 54 | nb::class_(m, "Bound") 55 | .def_ro("min", &Bound::min) 56 | .def_ro("max", &Bound::max) 57 | .def_ro("t_min", &Bound::t_min) 58 | .def_ro("t_max", &Bound::t_max) 59 | .def("__repr__", [](const Bound& ext) { 60 | return "[" + std::to_string(ext.min) + ", " + std::to_string(ext.max) + "]"; 61 | }); 62 | 63 | nb::class_>(m, "Trajectory") 64 | .def(nb::init(), "dofs"_a) 65 | #if defined WITH_CLOUD_CLIENT 66 | .def(nb::init(), "dofs"_a, "max_number_of_waypoints"_a) 67 | #endif 68 | .def_ro("degrees_of_freedom", &Trajectory::degrees_of_freedom) 69 | .def_prop_ro("profiles", &Trajectory::get_profiles) 70 | .def_prop_ro("duration", &Trajectory::get_duration) 71 | .def_prop_ro("intermediate_durations", &Trajectory::get_intermediate_durations) 72 | .def_prop_ro("independent_min_durations", &Trajectory::get_independent_min_durations) 73 | .def_prop_ro("position_extrema", &Trajectory::get_position_extrema) 74 | .def("at_time", [](const Trajectory& traj, double time, bool return_section=false) { 75 | std::vector new_position(traj.degrees_of_freedom), new_velocity(traj.degrees_of_freedom), new_acceleration(traj.degrees_of_freedom), new_jerk(traj.degrees_of_freedom); 76 | size_t new_section; 77 | traj.at_time(time, new_position, new_velocity, new_acceleration, new_jerk, new_section); 78 | if (return_section) { 79 | return nb::make_tuple(new_position, new_velocity, new_acceleration, new_section); 80 | } 81 | return nb::make_tuple(new_position, new_velocity, new_acceleration); 82 | }, "time"_a, "return_section"_a=false) 83 | .def("get_first_time_at_position", &Trajectory::get_first_time_at_position, "dof"_a, "position"_a, "time_after"_a=0.0); 84 | 85 | nb::class_>(m, "InputParameter") 86 | .def(nb::init(), "dofs"_a) 87 | #if defined WITH_CLOUD_CLIENT 88 | .def(nb::init(), "dofs"_a, "max_number_of_waypoints"_a) 89 | #endif 90 | .def_ro("degrees_of_freedom", &InputParameter::degrees_of_freedom) 91 | .def_rw("current_position", &InputParameter::current_position) 92 | .def_rw("current_velocity", &InputParameter::current_velocity) 93 | .def_rw("current_acceleration", &InputParameter::current_acceleration) 94 | .def_rw("target_position", &InputParameter::target_position) 95 | .def_rw("target_velocity", &InputParameter::target_velocity) 96 | .def_rw("target_acceleration", &InputParameter::target_acceleration) 97 | .def_rw("max_velocity", &InputParameter::max_velocity) 98 | .def_rw("max_acceleration", &InputParameter::max_acceleration) 99 | .def_rw("max_jerk", &InputParameter::max_jerk) 100 | .def_rw("min_velocity", &InputParameter::min_velocity, nb::arg().none()) 101 | .def_rw("min_acceleration", &InputParameter::min_acceleration, nb::arg().none()) 102 | .def_rw("intermediate_positions", &InputParameter::intermediate_positions) 103 | .def_rw("per_section_max_velocity", &InputParameter::per_section_max_velocity, nb::arg().none()) 104 | .def_rw("per_section_max_acceleration", &InputParameter::per_section_max_acceleration, nb::arg().none()) 105 | .def_rw("per_section_max_jerk", &InputParameter::per_section_max_jerk, nb::arg().none()) 106 | .def_rw("per_section_min_velocity", &InputParameter::per_section_min_velocity, nb::arg().none()) 107 | .def_rw("per_section_min_acceleration", &InputParameter::per_section_min_acceleration, nb::arg().none()) 108 | .def_rw("per_section_max_position", &InputParameter::per_section_max_position, nb::arg().none()) 109 | .def_rw("per_section_min_position", &InputParameter::per_section_min_position, nb::arg().none()) 110 | .def_rw("max_position", &InputParameter::max_position, nb::arg().none()) 111 | .def_rw("min_position", &InputParameter::min_position, nb::arg().none()) 112 | .def_rw("enabled", &InputParameter::enabled) 113 | .def_rw("control_interface", &InputParameter::control_interface) 114 | .def_rw("synchronization", &InputParameter::synchronization) 115 | .def_rw("duration_discretization", &InputParameter::duration_discretization) 116 | .def_rw("per_dof_control_interface", &InputParameter::per_dof_control_interface, nb::arg().none()) 117 | .def_rw("per_dof_synchronization", &InputParameter::per_dof_synchronization, nb::arg().none()) 118 | .def_rw("minimum_duration", &InputParameter::minimum_duration, nb::arg().none()) 119 | .def_rw("per_section_minimum_duration", &InputParameter::per_section_minimum_duration, nb::arg().none()) 120 | .def_rw("interrupt_calculation_duration", &InputParameter::interrupt_calculation_duration, nb::arg().none()) 121 | .def("validate", &InputParameter::validate, "check_current_state_within_limits"_a=false, "check_target_state_within_limits"_a=true) 122 | .def(nb::self != nb::self) 123 | .def("__repr__", &InputParameter::to_string); 124 | 125 | nb::class_>(m, "OutputParameter") 126 | .def(nb::init(), "dofs"_a) 127 | #if defined WITH_CLOUD_CLIENT 128 | .def(nb::init(), "dofs"_a, "max_number_of_waypoints"_a) 129 | #endif 130 | .def_ro("degrees_of_freedom", &OutputParameter::degrees_of_freedom) 131 | .def_ro("new_position", &OutputParameter::new_position) 132 | .def_ro("new_velocity", &OutputParameter::new_velocity) 133 | .def_ro("new_acceleration", &OutputParameter::new_acceleration) 134 | .def_ro("new_jerk", &OutputParameter::new_jerk) 135 | .def_ro("new_section", &OutputParameter::new_section) 136 | .def_ro("did_section_change", &OutputParameter::did_section_change) 137 | .def_ro("trajectory", &OutputParameter::trajectory) 138 | .def_rw("time", &OutputParameter::time) 139 | .def_ro("new_calculation", &OutputParameter::new_calculation) 140 | .def_ro("was_calculation_interrupted", &OutputParameter::was_calculation_interrupted) 141 | .def_ro("calculation_duration", &OutputParameter::calculation_duration) 142 | .def("pass_to_input", &OutputParameter::pass_to_input, "input"_a) 143 | .def("__repr__", &OutputParameter::to_string) 144 | .def("__copy__", [](const OutputParameter &self) { 145 | return OutputParameter(self); 146 | }); 147 | 148 | nb::class_>(m, "Ruckig") 149 | .def(nb::init(), "dofs"_a) 150 | .def(nb::init(), "dofs"_a, "delta_time"_a) 151 | #if defined WITH_CLOUD_CLIENT 152 | .def(nb::init(), "dofs"_a, "delta_time"_a, "max_number_of_waypoints"_a=0) 153 | .def("filter_intermediate_positions", &RuckigThrow::filter_intermediate_positions, "input"_a, "threshold_distance"_a) 154 | #endif 155 | .def_ro("max_number_of_waypoints", &RuckigThrow::max_number_of_waypoints) 156 | .def_ro("degrees_of_freedom", &RuckigThrow::degrees_of_freedom) 157 | .def_rw("delta_time", &RuckigThrow::delta_time) 158 | .def("reset", &RuckigThrow::reset) 159 | .def("validate_input", &RuckigThrow::validate_input, "input"_a, "check_current_state_within_limits"_a=false, "check_target_state_within_limits"_a=true) 160 | .def("calculate", static_cast::*)(const InputParameter&, Trajectory&)>(&RuckigThrow::calculate), "input"_a, "trajectory"_a) 161 | .def("calculate", static_cast::*)(const InputParameter&, Trajectory&, bool&)>(&RuckigThrow::calculate), "input"_a, "trajectory"_a, "was_interrupted"_a) 162 | .def("update", static_cast::*)(const InputParameter&, OutputParameter&)>(&RuckigThrow::update), "input"_a, "output"_a); 163 | 164 | nb::class_(m, "BrakeProfile") 165 | .def_ro("duration", &BrakeProfile::duration) 166 | .def_ro("t", &BrakeProfile::t) 167 | .def_ro("j", &BrakeProfile::j) 168 | .def_ro("a", &BrakeProfile::a) 169 | .def_ro("v", &BrakeProfile::v) 170 | .def_ro("p", &BrakeProfile::p); 171 | 172 | nb::class_(m, "Profile") 173 | .def_ro("t", &Profile::t) 174 | .def_ro("j", &Profile::j) 175 | .def_ro("a", &Profile::a) 176 | .def_ro("v", &Profile::v) 177 | .def_ro("p", &Profile::p) 178 | .def_ro("brake", &Profile::brake) 179 | .def_ro("accel", &Profile::accel) 180 | .def_ro("pf", &Profile::pf) 181 | .def_ro("vf", &Profile::vf) 182 | .def_ro("af", &Profile::af) 183 | .def_ro("limits", &Profile::limits) 184 | .def_ro("direction", &Profile::direction) 185 | .def_ro("control_signs", &Profile::control_signs) 186 | .def("__repr__", &Profile::to_string); 187 | } 188 | -------------------------------------------------------------------------------- /src/ruckig/velocity_second_step1.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | namespace ruckig { 6 | 7 | VelocitySecondOrderStep1::VelocitySecondOrderStep1(double v0, double vf, double aMax, double aMin): _aMax(aMax), _aMin(aMin) { 8 | vd = vf - v0; 9 | } 10 | 11 | bool VelocitySecondOrderStep1::get_profile(const Profile& input, Block& block) { 12 | auto& p = block.p_min; 13 | p.set_boundary(input); 14 | 15 | const double af = (vd > 0) ? _aMax : _aMin; 16 | p.t[0] = 0; 17 | p.t[1] = vd / af; 18 | p.t[2] = 0; 19 | p.t[3] = 0; 20 | p.t[4] = 0; 21 | p.t[5] = 0; 22 | p.t[6] = 0; 23 | 24 | if (p.check_for_second_order_velocity(af)) { 25 | block.t_min = p.t_sum.back() + p.brake.duration + p.accel.duration; 26 | return true; 27 | } 28 | return false; 29 | } 30 | 31 | } // namespace ruckig 32 | -------------------------------------------------------------------------------- /src/ruckig/velocity_second_step2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | 7 | namespace ruckig { 8 | 9 | VelocitySecondOrderStep2::VelocitySecondOrderStep2(double tf, double v0, double vf, double aMax, double aMin): tf(tf), _aMax(aMax), _aMin(aMin) { 10 | vd = vf - v0; 11 | } 12 | 13 | bool VelocitySecondOrderStep2::get_profile(Profile& profile) { 14 | const double af = vd / tf; 15 | 16 | profile.t[0] = 0; 17 | profile.t[1] = tf; 18 | profile.t[2] = 0; 19 | profile.t[3] = 0; 20 | profile.t[4] = 0; 21 | profile.t[5] = 0; 22 | profile.t[6] = 0; 23 | 24 | if (profile.check_for_second_order_velocity_with_timing(tf, af, _aMax, _aMin)) { 25 | profile.pf = profile.p.back(); 26 | return true; 27 | } 28 | 29 | return false; 30 | } 31 | 32 | } // namespace ruckig 33 | -------------------------------------------------------------------------------- /src/ruckig/velocity_third_step1.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | namespace ruckig { 6 | 7 | VelocityThirdOrderStep1::VelocityThirdOrderStep1(double v0, double a0, double vf, double af, double aMax, double aMin, double jMax): a0(a0), af(af), _aMax(aMax), _aMin(aMin), _jMax(jMax) { 8 | vd = vf - v0; 9 | } 10 | 11 | void VelocityThirdOrderStep1::time_acc0(ProfileIter& profile, double aMax, double aMin, double jMax, bool) const { 12 | profile->t[0] = (-a0 + aMax)/jMax; 13 | profile->t[1] = (a0*a0 + af*af)/(2*aMax*jMax) - aMax/jMax + vd/aMax; 14 | profile->t[2] = (-af + aMax)/jMax; 15 | profile->t[3] = 0; 16 | profile->t[4] = 0; 17 | profile->t[5] = 0; 18 | profile->t[6] = 0; 19 | 20 | if (profile->check_for_velocity(jMax, aMax, aMin)) { 21 | add_profile(profile); 22 | } 23 | } 24 | 25 | void VelocityThirdOrderStep1::time_none(ProfileIter& profile, double aMax, double aMin, double jMax, bool return_after_found) const { 26 | double h1 = (a0*a0 + af*af)/2 + jMax*vd; 27 | if (h1 >= 0.0) { 28 | h1 = std::sqrt(h1); 29 | 30 | // Solution 1 31 | { 32 | profile->t[0] = -(a0 + h1)/jMax; 33 | profile->t[1] = 0; 34 | profile->t[2] = -(af + h1)/jMax; 35 | profile->t[3] = 0; 36 | profile->t[4] = 0; 37 | profile->t[5] = 0; 38 | profile->t[6] = 0; 39 | 40 | if (profile->check_for_velocity(jMax, aMax, aMin)) { 41 | add_profile(profile); 42 | if (return_after_found) { 43 | return; 44 | } 45 | } 46 | } 47 | 48 | // Solution 2 49 | { 50 | profile->t[0] = (-a0 + h1)/jMax; 51 | profile->t[1] = 0; 52 | profile->t[2] = (-af + h1)/jMax; 53 | profile->t[3] = 0; 54 | profile->t[4] = 0; 55 | profile->t[5] = 0; 56 | profile->t[6] = 0; 57 | 58 | if (profile->check_for_velocity(jMax, aMax, aMin)) { 59 | add_profile(profile); 60 | } 61 | } 62 | } 63 | } 64 | 65 | bool VelocityThirdOrderStep1::time_all_single_step(Profile* profile, double aMax, double aMin, double jMax) const { 66 | if (std::abs(af - a0) > DBL_EPSILON) { 67 | return false; 68 | } 69 | 70 | profile->t[0] = 0; 71 | profile->t[1] = 0; 72 | profile->t[2] = 0; 73 | profile->t[3] = 0; 74 | profile->t[4] = 0; 75 | profile->t[5] = 0; 76 | profile->t[6] = 0; 77 | 78 | if (std::abs(a0) > DBL_EPSILON) { 79 | profile->t[3] = vd / a0; 80 | if (profile->check_for_velocity(0.0, aMax, aMin)) { 81 | return true; 82 | } 83 | 84 | } else if (std::abs(vd) < DBL_EPSILON) { 85 | if (profile->check_for_velocity(0.0, aMax, aMin)) { 86 | return true; 87 | } 88 | } 89 | 90 | return false; 91 | } 92 | 93 | 94 | bool VelocityThirdOrderStep1::get_profile(const Profile& input, Block& block) { 95 | // Zero-limits special case 96 | if (_jMax == 0.0) { 97 | auto& p = block.p_min; 98 | p.set_boundary(input); 99 | 100 | if (time_all_single_step(&p, _aMax, _aMin, _jMax)) { 101 | block.t_min = p.t_sum.back() + p.brake.duration + p.accel.duration; 102 | if (std::abs(a0) > DBL_EPSILON) { 103 | block.a = Block::Interval(block.t_min, std::numeric_limits::infinity()); 104 | } 105 | return true; 106 | } 107 | return false; 108 | } 109 | 110 | const ProfileIter start = valid_profiles.begin(); 111 | ProfileIter profile = start; 112 | profile->set_boundary(input); 113 | 114 | if (std::abs(af) < DBL_EPSILON) { 115 | // There is no blocked interval when af==0, so return after first found profile 116 | const double aMax = (vd >= 0) ? _aMax : _aMin; 117 | const double aMin = (vd >= 0) ? _aMin : _aMax; 118 | const double jMax = (vd >= 0) ? _jMax : -_jMax; 119 | 120 | time_none(profile, aMax, aMin, jMax, true); 121 | if (profile > start) { goto return_block; } 122 | time_acc0(profile, aMax, aMin, jMax, true); 123 | if (profile > start) { goto return_block; } 124 | 125 | time_none(profile, aMin, aMax, -jMax, true); 126 | if (profile > start) { goto return_block; } 127 | time_acc0(profile, aMin, aMax, -jMax, true); 128 | 129 | } else { 130 | time_none(profile, _aMax, _aMin, _jMax, false); 131 | time_none(profile, _aMin, _aMax, -_jMax, false); 132 | time_acc0(profile, _aMax, _aMin, _jMax, false); 133 | time_acc0(profile, _aMin, _aMax, -_jMax, false); 134 | } 135 | 136 | return_block: 137 | return Block::calculate_block(block, valid_profiles, std::distance(start, profile)); 138 | } 139 | 140 | } // namespace ruckig 141 | -------------------------------------------------------------------------------- /src/ruckig/velocity_third_step2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | 7 | namespace ruckig { 8 | 9 | VelocityThirdOrderStep2::VelocityThirdOrderStep2(double tf, double v0, double a0, double vf, double af, double aMax, double aMin, double jMax): a0(a0), tf(tf), af(af), _aMax(aMax), _aMin(aMin), _jMax(jMax) { 10 | vd = vf - v0; 11 | ad = af - a0; 12 | } 13 | 14 | bool VelocityThirdOrderStep2::time_acc0(Profile& profile, double aMax, double aMin, double jMax) { 15 | // UD Solution 1/2 16 | { 17 | const double h1 = std::sqrt((-ad*ad + 2*jMax*((a0 + af)*tf - 2*vd))/(jMax*jMax) + tf*tf); 18 | 19 | profile.t[0] = ad/(2*jMax) + (tf - h1)/2; 20 | profile.t[1] = h1; 21 | profile.t[2] = tf - (profile.t[0] + h1); 22 | profile.t[3] = 0; 23 | profile.t[4] = 0; 24 | profile.t[5] = 0; 25 | profile.t[6] = 0; 26 | 27 | if (profile.check_for_velocity_with_timing(tf, jMax, aMax, aMin)) { 28 | profile.pf = profile.p.back(); 29 | return true; 30 | } 31 | } 32 | 33 | // UU Solution 34 | { 35 | const double h1 = (-ad + jMax*tf); 36 | 37 | profile.t[0] = -ad*ad/(2*jMax*h1) + (vd - a0*tf)/h1; 38 | profile.t[1] = -ad/jMax + tf; 39 | profile.t[2] = 0; 40 | profile.t[3] = 0; 41 | profile.t[4] = 0; 42 | profile.t[5] = 0; 43 | profile.t[6] = tf - (profile.t[0] + profile.t[1]); 44 | 45 | if (profile.check_for_velocity_with_timing(tf, jMax, aMax, aMin)) { 46 | profile.pf = profile.p.back(); 47 | return true; 48 | } 49 | } 50 | 51 | // UU Solution - 2 step 52 | { 53 | profile.t[0] = 0; 54 | profile.t[1] = -ad/jMax + tf; 55 | profile.t[2] = 0; 56 | profile.t[3] = 0; 57 | profile.t[4] = 0; 58 | profile.t[5] = 0; 59 | profile.t[6] = ad/jMax; 60 | 61 | if (profile.check_for_velocity_with_timing(tf, jMax, aMax, aMin)) { 62 | profile.pf = profile.p.back(); 63 | return true; 64 | } 65 | } 66 | 67 | return false; 68 | } 69 | 70 | bool VelocityThirdOrderStep2::time_none(Profile& profile, double aMax, double aMin, double jMax) { 71 | if (std::abs(a0) < DBL_EPSILON && std::abs(af) < DBL_EPSILON && std::abs(vd) < DBL_EPSILON) { 72 | profile.t[0] = 0; 73 | profile.t[1] = tf; 74 | profile.t[2] = 0; 75 | profile.t[3] = 0; 76 | profile.t[4] = 0; 77 | profile.t[5] = 0; 78 | profile.t[6] = 0; 79 | 80 | if (profile.check_for_velocity_with_timing(tf, jMax, aMax, aMin)) { 81 | profile.pf = profile.p.back(); 82 | return true; 83 | } 84 | } 85 | 86 | // UD Solution 1/2 87 | { 88 | const double h1 = 2*(af*tf - vd); 89 | 90 | profile.t[0] = h1/ad; 91 | profile.t[1] = tf - profile.t[0]; 92 | profile.t[2] = 0; 93 | profile.t[3] = 0; 94 | profile.t[4] = 0; 95 | profile.t[5] = 0; 96 | profile.t[6] = 0; 97 | 98 | const double jf = ad*ad/h1; 99 | 100 | if (std::abs(jf) < std::abs(jMax) + 1e-12 && profile.check_for_velocity_with_timing(tf, jf, aMax, aMin)) { 101 | profile.pf = profile.p.back(); 102 | return true; 103 | } 104 | } 105 | 106 | return false; 107 | } 108 | 109 | bool VelocityThirdOrderStep2::get_profile(Profile& profile) { 110 | // Test all cases to get ones that match 111 | // However we should guess which one is correct and try them first... 112 | if (vd > 0) { 113 | return check_all(profile, _aMax, _aMin, _jMax) || check_all(profile, _aMin, _aMax, -_jMax); 114 | } 115 | 116 | return check_all(profile, _aMin, _aMax, -_jMax) || check_all(profile, _aMax, _aMin, _jMax); 117 | } 118 | 119 | } // namespace ruckig 120 | -------------------------------------------------------------------------------- /test/benchmark_target.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "randomizer.hpp" 5 | 6 | #include 7 | 8 | 9 | using namespace ruckig; 10 | 11 | 12 | template 13 | double check_update(OTGType& otg, InputParameter& input) { 14 | OutputParameter output; 15 | const auto result = otg.update(input, output); 16 | return output.calculation_duration; 17 | } 18 | 19 | template 20 | double check_calculation(OTGType& otg, InputParameter& input) { 21 | Trajectory trajectory; 22 | const auto start = std::chrono::steady_clock::now(); 23 | const auto result = otg.calculate(input, trajectory); 24 | const auto stop = std::chrono::steady_clock::now(); 25 | return std::chrono::duration_cast(stop - start).count() / 1000.0; 26 | } 27 | 28 | 29 | std::tuple analyze(const std::vector& v) { 30 | const double sum = std::accumulate(v.begin(), v.end(), 0.0); 31 | const double mean = sum / v.size(); 32 | 33 | std::vector diff(v.size()); 34 | std::transform(v.begin(), v.end(), diff.begin(), [mean](double x) { return x - mean; }); 35 | const double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0); 36 | const double std_deviation = std::sqrt(sq_sum / v.size()); 37 | return std::make_tuple(mean, std_deviation); 38 | } 39 | 40 | 41 | template 42 | void benchmark(size_t n, double number_trajectories, bool verbose = true) { 43 | OTGType otg {0.005}; 44 | 45 | std::normal_distribution position_dist {0.0, 4.0}; 46 | std::normal_distribution dynamic_dist {0.0, 0.8}; 47 | std::uniform_real_distribution limit_dist {0.1, 12.0}; 48 | 49 | Randomizer p { position_dist, 42 }; 50 | Randomizer d { dynamic_dist, 43 }; 51 | Randomizer l { limit_dist, 44 }; 52 | 53 | InputParameter input; 54 | // input.synchronization = Synchronization::None; 55 | // input.control_interface = ControlInterface::Velocity; 56 | std::vector average, worst, global; 57 | 58 | // Initial warm-up calculation 59 | // p.fill(input.current_position); 60 | // p.fill(input.target_position); 61 | // l.fill(input.max_velocity, input.target_velocity); 62 | // l.fill(input.max_acceleration, input.target_acceleration); 63 | // l.fill(input.max_jerk); 64 | // check_calculation(otg, input); 65 | 66 | for (size_t j = 0; j < n; ++j) { 67 | double average_ = 0.0; 68 | double worst_ = 0.0; 69 | size_t n {1}; 70 | 71 | const auto start = std::chrono::steady_clock::now(); 72 | 73 | for (size_t i = 0; i < number_trajectories; ++i) { 74 | p.fill(input.current_position); 75 | d.fill_or_zero(input.current_velocity, 0.9); 76 | d.fill_or_zero(input.current_acceleration, 0.8); 77 | p.fill(input.target_position); 78 | d.fill_or_zero(input.target_velocity, 0.7); 79 | if constexpr (std::is_same>::value) { 80 | d.fill_or_zero(input.target_acceleration, 0.6); 81 | } 82 | 83 | l.fill(input.max_velocity, input.target_velocity); 84 | l.fill(input.max_acceleration, input.target_acceleration); 85 | l.fill(input.max_jerk); 86 | 87 | // input.current_velocity[0] = 0.5; 88 | // input.target_velocity[0] = 0.5; 89 | // input.target_position[0] = input.current_position[0] + 1.0; 90 | // input.max_jerk[0] = 0.0; 91 | 92 | if constexpr (std::is_same>::value) { 93 | if (!otg.template validate_input(input)) { 94 | continue; 95 | } 96 | } 97 | 98 | const double time = check_calculation(otg, input); 99 | average_ = average_ + (time - average_) / n; 100 | worst_ = std::max(worst_, time); 101 | ++n; 102 | } 103 | 104 | const auto stop = std::chrono::steady_clock::now(); 105 | const double global_ = std::chrono::duration_cast(stop - start).count() / 1000.0 / number_trajectories; 106 | 107 | average.emplace_back(average_); 108 | worst.emplace_back(worst_); 109 | global.emplace_back(global_); 110 | } 111 | 112 | const auto [average_mean, average_std] = analyze(average); 113 | const auto [worst_mean, worst_std] = analyze(worst); 114 | const auto [global_mean, global_std] = analyze(global); 115 | 116 | if (verbose) { 117 | std::cout << "---" << std::endl; 118 | std::cout << "Benchmark for " << otg.degrees_of_freedom << " DoFs on " << number_trajectories << " trajectories" << std::endl; 119 | std::cout << "Average Calculation Duration " << average_mean << " pm " << average_std << " [µs]" << std::endl; 120 | std::cout << "Worst Calculation Duration " << worst_mean << " pm " << worst_std << " [µs]" << std::endl; 121 | std::cout << "End-to-end Calculation Duration " << global_mean << " pm " << global_std << " [µs]" << std::endl; 122 | } 123 | 124 | // std::cout << otg.degrees_of_freedom << "\t" << average_mean << "\t" << average_std << "\t" << worst_mean << "\t" << worst_std << std::endl; 125 | } 126 | 127 | 128 | int main() { 129 | const size_t n {2 * 5}; // Number of iterations 130 | const size_t number_trajectories {4 * 64 * 1024}; 131 | 132 | std::cout << "Ruckig size: " << sizeof(Ruckig<3>) << " Byte" << std::endl; 133 | std::cout << "Trajectory<3> size: " << sizeof(Trajectory<3>) << " Byte" << std::endl; 134 | 135 | const size_t DOFs {3}; 136 | benchmark>(n, number_trajectories); 137 | } 138 | -------------------------------------------------------------------------------- /test/randomizer.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | 10 | template 11 | class Randomizer { 12 | template using Vector = ruckig::StandardVector; 13 | 14 | std::default_random_engine gen; 15 | T dist; 16 | std::uniform_real_distribution uniform_dist; 17 | 18 | public: 19 | explicit Randomizer() { } 20 | explicit Randomizer(T dist, int local_seed): dist(dist), uniform_dist(std::uniform_real_distribution(0.0, 1.0)) { 21 | gen.seed(local_seed); 22 | } 23 | 24 | void fill(Vector& input) { 25 | for (size_t dof = 0; dof < input.size(); ++dof) { 26 | input[dof] = dist(gen); 27 | } 28 | } 29 | 30 | void fill_or_zero(Vector& input, double p) { 31 | for (size_t dof = 0; dof < input.size(); ++dof) { 32 | input[dof] = uniform_dist(gen) < p ? dist(gen) : 0.0; 33 | } 34 | } 35 | 36 | void fill(Vector& input, const Vector& offset) { 37 | for (size_t dof = 0; dof < input.size(); ++dof) { 38 | input[dof] = dist(gen) + std::abs(offset[dof]); 39 | } 40 | } 41 | 42 | void fill_min(Vector& input, const Vector& offset) { 43 | for (size_t dof = 0; dof < input.size(); ++dof) { 44 | input[dof] = dist(gen) - std::abs(offset[dof]); 45 | } 46 | } 47 | }; 48 | -------------------------------------------------------------------------------- /third_party/doctest/LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2016-2023 Viktor Kirilov 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /third_party/httplib/LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2017 yhirose 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | 23 | -------------------------------------------------------------------------------- /third_party/nlohmann/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2013-2022 Niels Lohmann 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | --------------------------------------------------------------------------------