diff --git a/.circleci/README.md b/.circleci/README.md new file mode 100644 index 00000000..cbc2dd08 --- /dev/null +++ b/.circleci/README.md @@ -0,0 +1,28 @@ +# CI + +We have the following CI workflows in CirclCI. + +## integrate + +The main workflow for new commits. The following jobs are done: +- Check project codestyle. +- Build and test toppra on Python 3. +- Build and test toppra (C++). + +## release + +Check release branch for proper version. + + +## publish + +Publish source code to venues (PyPI). NOTE: This is currently not done. + + +# Github Action +Some tasks are done directly with Github Action. + +- Linting for C++ code with clang-tidy. + + Note: `cppcoreguidelines-pro-bounds-array-to-pointer-decay` leads to false reported error, thus disabled. + diff --git a/.circleci/config.yml b/.circleci/config.yml index 6e2ebcbd..fae2368b 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -1,4 +1,5 @@ version: 2 + jobs: check-codestyle: docker: @@ -34,7 +35,7 @@ jobs: path: test-reports destination: test-reports - test-python-3: + build-python-3: docker: - image: hungpham2511/toppra-dep:0.0.3 @@ -51,7 +52,7 @@ jobs: - run: name: install dependencies command: | - python3 -m virtualenv --python python3 venv3 && . venv3/bin/activate + virtualenv --python python3.7 venv3 && . venv3/bin/activate pip install invoke pathlib2 numpy cython invoke install-solvers pip install -e .[dev] @@ -66,12 +67,13 @@ jobs: command: | . venv3/bin/activate mkdir test-reports - pytest -q tests --durations=3 --junitxml=test-reports/junit.xml + python --version + python -m pytest -q tests --durations=3 --junitxml=test-reports/junit.xml - store_test_results: path: test-reports - test-python-2: + build-cpp: docker: - image: hungpham2511/toppra-dep:0.0.3 @@ -79,40 +81,31 @@ jobs: steps: - checkout - - restore_cache: - keys: - - v1-dependencies2-{{ checksum "requirements.txt" }} - - run: - name: install dependencies + name: dependencies command: | - python -m pip install virtualenv --user - python -m virtualenv venv && . venv/bin/activate - python -m pip install invoke pathlib2 numpy cython - python -m invoke install-solvers - pip install -e .[dev] - - - save_cache: - paths: - - ./venv - key: v1-dependencies2-{{ checksum "requirements.txt" }} + sudo apt install -y curl + echo "deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -sc) robotpkg" \ + | sudo tee /etc/apt/sources.list.d/robotpkg.list + curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key \ + | sudo apt-key add - + sudo apt update + sudo apt install -y clang-tidy libeigen3-dev robotpkg-pinocchio robotpkg-qpoases libglpk-dev - run: - name: test + name: build command: | - . venv/bin/activate - export PYTHONPATH=$PYTHONPATH:`openrave-config --python-dir` - mkdir test-reports - pytest --durations=3 --junitxml=test-reports/junit.xml + export LD_LIBRARY_PATH=/opt/openrobots/lib:${LD_LIBRARY_PATH} + export CMAKE_PREFIX_PATH=/opt/openrobots + mkdir build && cd build && cmake -DBUILD_WITH_PINOCCHIO=ON -DBUILD_WITH_qpOASES=ON -DBUILD_WITH_GLPK=ON .. + make -j4 + working_directory: ~/repo/cpp - run: - name: Collect log data + name: test command: | - cp /tmp/toppra.log ~/repo/test-reports/ - - - store_test_results: - path: test-reports - destination: test-reports + ./tests/all_tests + working_directory: ~/repo/cpp/build pre-release: docker: @@ -147,11 +140,14 @@ jobs: workflows: version: 2 - test-and-lint: + integrate: jobs: - - test-python-2 - - test-python-3 + - build-cpp - check-codestyle + - build-python-3: + requires: + - check-codestyle + release: jobs: - pre-release: @@ -160,6 +156,9 @@ workflows: only: - fix-ci - /release-.*/ + + publish: + jobs: - release: filters: branches: diff --git a/.clang-format b/.clang-format new file mode 100644 index 00000000..707b82b8 --- /dev/null +++ b/.clang-format @@ -0,0 +1,117 @@ +--- +Language: Cpp +# BasedOnStyle: Google +AccessModifierOffset: -1 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlines: Left +AlignOperands: true +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: All +AllowShortIfStatementsOnASingleLine: true +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: true +BinPackArguments: true +BinPackParameters: true +BraceWrapping: + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Attach +BreakBeforeInheritanceComma: false +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +BreakConstructorInitializers: BeforeColon +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 88 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: true +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IncludeBlocks: Preserve +IncludeCategories: + - Regex: '^' + Priority: 2 + - Regex: '^<.*\.h>' + Priority: 1 + - Regex: '^<.*' + Priority: 2 + - Regex: '.*' + Priority: 3 +IncludeIsMainRegex: '([-_](test|unittest))?$' +IndentCaseLabels: true +IndentPPDirectives: None +IndentWidth: 2 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: false +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBlockIndentWidth: 2 +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: false +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Left +RawStringFormats: + - Delimiter: pb + Language: TextProto + BasedOnStyle: google +ReflowComments: true +SortIncludes: true +SortUsingDeclarations: true +SpaceAfterCStyleCast: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: ControlStatements +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Auto +TabWidth: 8 +UseTab: Never +... + diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md new file mode 100644 index 00000000..d399bf21 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -0,0 +1,25 @@ +--- +name: Bug report +about: Create a report to help us improve +title: '' +labels: 'Type: bug' +assignees: '' + +--- + +**Describe the bug** +A clear and concise description of what the bug is. + +**To Reproduce** +Steps to reproduce the behavior: +1. +2. + +**Expected behavior** +A clear and concise description of what you expected to happen. + +**Screenshots** +If applicable, add screenshots to help explain your problem. + +**Version** +Branch, commit, version? diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md new file mode 100644 index 00000000..bbcbbe7d --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature_request.md @@ -0,0 +1,20 @@ +--- +name: Feature request +about: Suggest an idea for this project +title: '' +labels: '' +assignees: '' + +--- + +**Is your feature request related to a problem? Please describe.** +A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] + +**Describe the solution you'd like** +A clear and concise description of what you want to happen. + +**Describe alternatives you've considered** +A clear and concise description of any alternative solutions or features you've considered. + +**Additional context** +Add any other context or screenshots about the feature request here. diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md new file mode 100644 index 00000000..390cb882 --- /dev/null +++ b/.github/pull_request_template.md @@ -0,0 +1,8 @@ +Fixes # + +Changes in this PRs: +- +- + +Checklists: +- [ ] Update CHANGELOG.md with a single line describing this PR diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml new file mode 100644 index 00000000..799813c9 --- /dev/null +++ b/.github/workflows/main.yml @@ -0,0 +1,35 @@ +name: Lint (C++, clang-tidy) + +on: + push: + branches: [ develop ] + pull_request: + branches: [ develop ] + +jobs: + lint: + runs-on: ubuntu-latest + + steps: + # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it + - uses: actions/checkout@v2 + + - name: Install deps + run: | + sudo apt install -y curl + echo "deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -sc) robotpkg" \ + | sudo tee /etc/apt/sources.list.d/robotpkg.list + curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key \ + | sudo apt-key add - + sudo apt update + sudo apt install -y clang-tidy libeigen3-dev robotpkg-pinocchio robotpkg-qpoases + + - name: Lint + run: | + cd $GITHUB_WORKSPACE/cpp + export LD_LIBRARY_PATH=/opt/openrobots/lib:${LD_LIBRARY_PATH} + export CMAKE_PREFIX_PATH=/opt/openrobots + mkdir build && cd build && cmake -DBUILD_WITH_PINOCCHIO=ON -DBUILD_WITH_qpOASES=ON -DCMAKE_EXPORT_COMPILE_COMMANDS=1 .. + clang-tidy src/**/*.cpp \ + -checks=clang-analyzer-*,clang-analyzer-cplusplus*,cppcoreguidelines-*,performance-*,modernize-*,readability-,-cppcoreguidelines-pro-bounds-array-to-pointer-decay\ + -warnings-as-errors=clang-analyzer-*,clang-analyzer-cplusplus*,cppcoreguidelines-*,modernize-* diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 00000000..e69de29b diff --git a/.pylintrc b/.pylintrc index 5d8790db..fb091b2c 100644 --- a/.pylintrc +++ b/.pylintrc @@ -142,6 +142,7 @@ disable= xrange-builtin, xreadlines-attribute, zip-builtin-not-iterating, + bad-continuation # Enable the message, report, category or checker with the given id(s). You can # either give multiple identifier separated by comma (,) or put this option diff --git a/HISTORY.md b/HISTORY.md new file mode 100644 index 00000000..5552deba --- /dev/null +++ b/HISTORY.md @@ -0,0 +1,37 @@ +# History + +## 0.3.0 (May 3 2020) + +Major release! Implement TOPPRA in C++ and several improvements to Python codebase. + +### Added + +- [cpp] Add solver wrapper based on GLPK. +- [cpp] Initial cpp TOPPRA implementation: compute parametrization, feasible sets, qpOASES solver wrapper. +- [python] Implement a new trajectory class for specified velocities. + +### Changed + +- [python] Improve documentation for `toppra.constraint` +- [python] #98: Eliminate use of deprecated method. +- [cpp] Bug fixes in solver wrapper. +- [python] Simplify TOPPRA class interface. +- [python] [0e022c][cm-0e022c] Update README.md to reflect development roadmap. +- [python] Format some source files with black. +- [python] [#78][gh-78] Improve documentation structure. +- [python] [#79][gh-79] Improve documentation structure. + +### Removed + +- Dropping support for Python2.7. Adding type annotation compatible to Python 3. + +## 0.2.3 (Jan 25 2020) + +- Auto-gridpoints feature (#73) + +## <0.2.3 +- Many commits and features. + +[gh-78]: https://github.com/hungpham2511/toppra/pull/78 +[gh-79]: https://github.com/hungpham2511/toppra/pull/79 +[cm-0e022c]: https://github.com/hungpham2511/toppra/commit/0e022c53ab9db473485bd9fb6b8f34a7364efdf8 diff --git a/Makefile b/Makefile index 2a255c7e..36da8e5c 100644 --- a/Makefile +++ b/Makefile @@ -3,10 +3,13 @@ lint: pycodestyle toppra --max-line-length=120 --ignore=E731,W503 pydocstyle toppra -doc: +DOC: echo "Buidling toppra docs" sphinx-build -b html docs/source docs/build +gen-tags: + ctags -Re --exclude='.tox' --exclude='venv' --exclude='docs' --exclude=='dist' . + coverage: python -m pytest -q --cov-report term --cov-report xml --cov=toppra tests diff --git a/README.md b/README.md index 277e1218..92756332 100644 --- a/README.md +++ b/README.md @@ -1,23 +1,36 @@ -# `TOPP-RA` +# `toppra`: Time-Optimal Path Parameterization [![CircleCI](https://circleci.com/gh/hungpham2511/toppra/tree/develop.svg?style=svg)](https://circleci.com/gh/hungpham2511/toppra/tree/develop) [![Coverage Status](https://coveralls.io/repos/github/hungpham2511/toppra/badge.svg?branch=master)](https://coveralls.io/github/hungpham2511/toppra?branch=master) [![Documentation Status](https://readthedocs.org/projects/toppra/badge/?version=latest)](https://toppra.readthedocs.io/en/latest/?badge=latest) -**TOPP-RA** is a library for computing the time-optimal path parametrization for robots subject to kinematic and dynamic constraints. -In general, given the inputs: -1. a geometric path `p(s)`, `s` in `[0, s_end]` ; -2. a list of constraints on joint velocity, joint accelerations, tool Cartesian velocity, et cetera. +- [Overview](#overview) +- [Development roadmap](#development-roadmap) +- [Supports](#supports) +- [Citing `toppra`](#citing--toppra-) -**TOPP-RA** returns the time-optimal path parameterization: `s_dot (s)`, from which the fastest trajectory `q(t)` that satisfies the given -constraints can be found. -**Documentation and tutorials** are available at (https://toppra.readthedocs.io/en/latest/index.html). +# Overview +**toppra** is a library for computing the time-optimal path +parametrization for robots subject to kinematic and dynamic +constraints. In general, given the inputs: -## Quick-start +1. a geometric path `p(s)`, `s` in `[0, s_end]`; +2. a list of constraints on joint velocity, joint accelerations, tool + Cartesian velocity, et cetera. -To install **TOPP-RA**, simply clone the repo and install with pip +**toppra** returns the time-optimal path parameterization: `s_dot +(s)`, from which the fastest trajectory `q(t)` that satisfies the +given constraints can be found. + +**Documentation and tutorials** are available at +(https://toppra.readthedocs.io/en/latest/index.html). + + +## Installation (Python) + +To install the latest development version, simply clone the repo and install with pip ``` shell git clone https://github.com/hungpham2511/toppra @@ -29,21 +42,47 @@ To install depencidencies for development, replace the second command with: cd toppra && pip install -e .[dev] ``` -## Citing **TOPP-RA** -If you use this library for your research, we encourage you to +## C++ API -1. reference the accompanying paper [«A new approach to Time-Optimal Path Parameterization based on Reachability Analysis»](https://www.researchgate.net/publication/318671280_A_New_Approach_to_Time-Optimal_Path_Parameterization_Based_on_Reachability_Analysis), *IEEE Transactions on Robotics*, vol. 34(3), pp. 645–659, 2018. -2. put a star on this repository. +`toppra` is also implemented as a stand-alone C++ library with minimal +dependency. See `cpp/README.md` for installation instruction and the +test cases for example usage. Be warned that this implementation is +relatively new and is not yet stable enough for production use. + +# Development roadmap +The following is a non-exhautive list of features that we are +considering to include in the library. -## Bug reports and supports -Please report any issues, questions via [Github issues tracker](https://github.com/hungpham2511/toppra/issues). +- Improve the trajectory / geometric path interface [#81](https://github.com/hungpham2511/toppra/issues/81) +- Implement a C++ interface [#43](https://github.com/hungpham2511/toppra/issues/43) +- Implement a C++ interface to popular motion planning libraries. +- Improve the numerical stability of the solvers for degenerate cases. +- Post-processing of output trajectories: [#56](https://github.com/hungpham2511/toppra/issues/56), [#80](https://github.com/hungpham2511/toppra/issues/80) -It will be very helpful if you can provide more details on the -errors/bugs that you encounter. In fact, the best way is to provide a -Minimal Working Example that produces the reported bug and attach it -with the issue report. +# Supports + +## Bug tracking +Please report any issues, questions or feature request via +[Github issues tracker](https://github.com/hungpham2511/toppra/issues). + +Please provide more details on the errors/bugs that you encounter. The +best way is to provide a Minimal Working Example that produces the +reported bug and attach it with the issue report. ## Contributions +Pull Requests are welcomed! +- Go ahead and create a Pull Request and we will review your proposal! +- For new features, or bug fixes, preferably the request should + contain unit tests. Note that `toppra` uses + [pytest](https://docs.pytest.org/en/latest/contents.html) for all + tests. Check out the test folder for more details. + + +# Citing `toppra` +If you use this library for your research, we encourage you to + +1. reference the accompanying paper [«A new approach to Time-Optimal Path Parameterization based on Reachability Analysis»](https://www.researchgate.net/publication/318671280_A_New_Approach_to_Time-Optimal_Path_Parameterization_Based_on_Reachability_Analysis), + *IEEE Transactions on Robotics*, vol. 34(3), pp. 645–659, 2018. +2. put a star on this repository. -Pull Requests are welcomed! Go ahead and create a Pull Request and we will review your proposal! For new features, or bug fixes, preferably the request should contain unit tests. Note that **TOPP-RA** uses [pytest](https://docs.pytest.org/en/latest/contents.html) for all tests. Check out the test folder for more details. diff --git a/VERSION b/VERSION index 373f8c6f..9325c3cc 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -0.2.3 \ No newline at end of file +0.3.0 \ No newline at end of file diff --git a/cpp/CMakeLists.txt b/cpp/CMakeLists.txt new file mode 100644 index 00000000..1ce6f04a --- /dev/null +++ b/cpp/CMakeLists.txt @@ -0,0 +1,97 @@ +cmake_minimum_required(VERSION 3.5) + +project(toppra + VERSION 0.3.0 + # Disable because not available in CMake 3.5 + #DESCRIPTION "Library computing the time-optimal path parameterization." + LANGUAGES CXX) +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD_REQUIRED True) +# Dynamics +option(BUILD_WITH_PINOCCHIO "Compile with Pinocchio library" OFF) +# Solvers +option(BUILD_WITH_qpOASES "Compile the wrapper for qpOASES" OFF) +option(BUILD_WITH_GLPK "Compile the wrapper for GLPK (GPL license)" OFF) +# General options +option(TOPPRA_DEBUG_ON "Set logging mode to on." OFF) + +find_package (Threads) + +find_package (Eigen3 REQUIRED) +message(STATUS "Found Eigen version ${EIGEN3_VERSION_STRING}") + +list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake") +if(BUILD_WITH_PINOCCHIO) + find_package(pinocchio REQUIRED) + message(STATUS "Found pinocchio ${pinocchio_VERSION}") +endif() +if(BUILD_WITH_qpOASES) + find_package(qpOASES REQUIRED) + message(STATUS "Found qpOASES") +endif() +if(BUILD_WITH_GLPK) + find_package(GLPK REQUIRED) + message(STATUS "Found glpk ${GLPK_LIBRARY} ${GLPK_INCLUDE_DIR}") +endif(BUILD_WITH_GLPK) + +add_subdirectory(src) +add_subdirectory(doc) + +# Download and unpack googletest at configure time +# source: https://github.com/google/googletest/tree/master/googletest +configure_file(CMakeLists.txt.in googletest-download/CMakeLists.txt) +execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" . + RESULT_VARIABLE result + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/googletest-download ) +if(result) + message(FATAL_ERROR "CMake step for googletest failed: ${result}") +endif() +execute_process(COMMAND ${CMAKE_COMMAND} --build . + RESULT_VARIABLE result + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/googletest-download ) +if(result) + message(FATAL_ERROR "Build step for googletest failed: ${result}") +endif() + +# Prevent overriding the parent project's compiler/linker +# settings on Windows +set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) + +# Add googletest directly to our build. This defines +# the gtest and gtest_main targets. +add_subdirectory(${CMAKE_CURRENT_BINARY_DIR}/googletest-src + ${CMAKE_CURRENT_BINARY_DIR}/googletest-build + EXCLUDE_FROM_ALL) + +# The gtest/gtest_main targets carry header search path +# dependencies automatically when using CMake 2.8.11 or +# later. Otherwise we have to add them here ourselves. +if (CMAKE_VERSION VERSION_LESS 2.8.11) + include_directories("${gtest_SOURCE_DIR}/include") +endif() + +enable_testing() +add_subdirectory(tests) + +# Generate and install CMake config, version and target files. +include(CMakePackageConfigHelpers) +set(CONFIG_INSTALL_DIR "lib/cmake/${PROJECT_NAME}") +set(GENERATED_DIR "${CMAKE_CURRENT_BINARY_DIR}/generated") +set(VERSION_CONFIG "${GENERATED_DIR}/${PROJECT_NAME}ConfigVersion.cmake") +set(PROJECT_CONFIG "${GENERATED_DIR}/${PROJECT_NAME}Config.cmake") +write_basic_package_version_file( + "${VERSION_CONFIG}" VERSION ${PROJECT_VERSION} COMPATIBILITY SameMajorVersion +) + +configure_package_config_file( + "cmake/Config.cmake.in" + "${PROJECT_CONFIG}" + INSTALL_DESTINATION "${CONFIG_INSTALL_DIR}" +) + +install(FILES "${PROJECT_CONFIG}" "${VERSION_CONFIG}" + DESTINATION "${CONFIG_INSTALL_DIR}") +install(EXPORT toppra::toppra + NAMESPACE toppra:: + FILE toppraTargets.cmake + DESTINATION "${CONFIG_INSTALL_DIR}") diff --git a/cpp/CMakeLists.txt.in b/cpp/CMakeLists.txt.in new file mode 100644 index 00000000..c6247af5 --- /dev/null +++ b/cpp/CMakeLists.txt.in @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 2.8.2) + +project(googletest-download NONE) + +include(ExternalProject) +ExternalProject_Add(googletest + GIT_REPOSITORY https://github.com/google/googletest.git + GIT_TAG master + SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-src" + BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-build" + CONFIGURE_COMMAND "" + BUILD_COMMAND "" + INSTALL_COMMAND "" + TEST_COMMAND "" +) diff --git a/cpp/README.md b/cpp/README.md new file mode 100644 index 00000000..e3a5e5e6 --- /dev/null +++ b/cpp/README.md @@ -0,0 +1,34 @@ +# Description +This is a implementation of the TOPP-RA algorithm. + +# Building Tests + +```sh +# clone +git clone -b develop https://github.com/hungpham2511/toppra + +# build +export LD_LIBRARY_PATH=/opt/openrobots/lib:${LD_LIBRARY_PATH} +export CMAKE_PREFIX_PATH=/opt/openrobots +mkdir build && cd build && cmake -DBUILD_WITH_PINOCCHIO=ON -DBUILD_WITH_qpOASES=ON .. +make -j4 + +# run test +./tests/all_tests +``` + +# Using TOPPRA in CMake-based project + +In your CMakeLists.txt, +```cmake +# The following line defines cmake target toppra::toppra +find_package(toppra) +... +target_link_library(foo PUBLIC toppra::toppra) +``` + +## How to install optional dependencies: + +- GLPK: `sudo apt install libglpk-dev` +- qpOASES: `sudo apt install robotpkg-qpoases` (follow http://robotpkg.openrobots.org/debian.html for robotpkg) +- pinocchio: `sudo apt install robotpkg-pinocchio` (follow http://robotpkg.openrobots.org/debian.html for robotpkg) diff --git a/cpp/cmake/Config.cmake.in b/cpp/cmake/Config.cmake.in new file mode 100644 index 00000000..e171c3e7 --- /dev/null +++ b/cpp/cmake/Config.cmake.in @@ -0,0 +1,13 @@ +set(TOPPRA_WITH_PINOCCHIO @BUILD_WITH_PINOCCHIO@) +set(TOPPRA_WITH_qpOASES @BUILD_WITH_qpOASES@) +set(TOPPRA_WITH_GLPK @BUILD_WITH_GLPK@) + +find_package(Eigen3 REQUIRED) + +if(TOPPRA_WITH_PINOCCHIO) + find_package(pinocchio REQUIRED) +endif() +# qpOASES and GLPK do not need to be found again, because +# they are not used as targets. + +include("${CMAKE_CURRENT_LIST_DIR}/toppraTargets.cmake") diff --git a/cpp/cmake/FindGLPK.cmake b/cpp/cmake/FindGLPK.cmake new file mode 100644 index 00000000..7b936a6e --- /dev/null +++ b/cpp/cmake/FindGLPK.cmake @@ -0,0 +1,25 @@ +# Try to find GLPK +# in standard prefixes and in ${GLPK_PREFIX} +# Once done this will define +# GLPK_FOUND - System has GLPK +# GLPK_INCLUDE_DIRS - The GLPK include directories +# GLPK_LIBRARIES - The libraries needed to use GLPK +# GLPK_DEFINITIONS - Compiler switches required for using GLPK + +FIND_PATH(GLPK_INCLUDE_DIR + NAMES glpk.h + PATHS ${GLPK_PREFIX} ${GLPK_PREFIX}/include + DOC "GLPK include directory") +FIND_LIBRARY(GLPK_LIBRARY + NAMES glpk + PATHS ${GLPK_PREFIX} ${GLPK_PREFIX}/lib + DOC "GLPK library") + +SET(GLPK_LIBRARIES ${GLPK_LIBRARY}) +SET(GLPK_INCLUDE_DIRS ${GLPK_INCLUDE_DIR}) + +# TODO Version could be extracted from the header. + +INCLUDE(FindPackageHandleStandardArgs) +FIND_PACKAGE_HANDLE_STANDARD_ARGS(GLPK DEFAULT_MSG GLPK_LIBRARY GLPK_INCLUDE_DIR) +mark_as_advanced(GLPK_INCLUDE_DIR GLPK_LIBRARY) diff --git a/cpp/cmake/FindqpOASES.cmake b/cpp/cmake/FindqpOASES.cmake new file mode 100644 index 00000000..ab0fb359 --- /dev/null +++ b/cpp/cmake/FindqpOASES.cmake @@ -0,0 +1,42 @@ +# +# Copyright 2019 CNRS +# +# Author: Guilhem Saurel +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU Lesser General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU Lesser General Public License for more details. +# +# You should have received a copy of the GNU Lesser General Public License +# along with this program. If not, see . +# + +# Try to find qpOASES +# in standard prefixes and in ${qpOASES_PREFIX} +# Once done this will define +# qpOASES_FOUND - System has qpOASES +# qpOASES_INCLUDE_DIRS - The qpOASES include directories +# qpOASES_LIBRARIES - The libraries needed to use qpOASES +# qpOASES_DEFINITIONS - Compiler switches required for using qpOASES + +FIND_PATH(qpOASES_INCLUDE_DIR + NAMES qpOASES.hpp + PATHS ${qpOASES_PREFIX} ${qpOASES_PREFIX}/include + ) +FIND_LIBRARY(qpOASES_LIBRARY + NAMES qpOASES + PATHS ${qpOASES_PREFIX} ${qpOASES_PREFIX}/lib + ) + +SET(qpOASES_LIBRARIES ${qpOASES_LIBRARY}) +SET(qpOASES_INCLUDE_DIRS ${qpOASES_INCLUDE_DIR}) + +INCLUDE(FindPackageHandleStandardArgs) +FIND_PACKAGE_HANDLE_STANDARD_ARGS(qpOASES DEFAULT_MSG qpOASES_LIBRARY qpOASES_INCLUDE_DIR) +mark_as_advanced(qpOASES_INCLUDE_DIR qpOASES_LIBRARY) diff --git a/cpp/doc/CMakeLists.txt b/cpp/doc/CMakeLists.txt new file mode 100644 index 00000000..d262fa11 --- /dev/null +++ b/cpp/doc/CMakeLists.txt @@ -0,0 +1,28 @@ +find_program(DOXYGEN doxygen) +IF(DOXYGEN) + option(INSTALL_DOCUMENTATION ON "Whether documentation should be installed") + + configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile + ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile + @ONLY) + + # Find all the public headers + set(HEADERS_DIR ${CMAKE_SOURCE_DIR}/src) + file(GLOB_RECURSE HEADERS ${HEADERS_DIR}/*.hpp) + + set(DOXYGEN_INDEX_FILE ${CMAKE_CURRENT_BINARY_DIR}/html/index.html) + add_custom_command(OUTPUT ${DOXYGEN_INDEX_FILE} + DEPENDS ${HEADERS} main.dox + COMMAND ${DOXYGEN} ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + MAIN_DEPENDENCY ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile + COMMENT "Generating documentation") + if(INSTALL_DOCUMENTATION) + add_custom_target(doc ALL DEPENDS ${DOXYGEN_INDEX_FILE}) + + install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/html + DESTINATION share/${PROJECT_NAME}/doc) + else() + add_custom_target(doc DEPENDS ${DOXYGEN_INDEX_FILE}) + endif() +ENDIF() diff --git a/cpp/doc/Doxyfile b/cpp/doc/Doxyfile new file mode 100644 index 00000000..7e2ec065 --- /dev/null +++ b/cpp/doc/Doxyfile @@ -0,0 +1,339 @@ +# Doxyfile 1.8.18 + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- +DOXYFILE_ENCODING = UTF-8 +PROJECT_NAME = "TOPP-RA" +PROJECT_NUMBER = +PROJECT_BRIEF = +PROJECT_LOGO = +OUTPUT_DIRECTORY = @CMAKE_BINARY_DIR@/doc +CREATE_SUBDIRS = NO +ALLOW_UNICODE_NAMES = NO +OUTPUT_LANGUAGE = English +OUTPUT_TEXT_DIRECTION = None +BRIEF_MEMBER_DESC = YES +REPEAT_BRIEF = YES +ABBREVIATE_BRIEF = "The $name class" \ + "The $name widget" \ + "The $name file" \ + is \ + provides \ + specifies \ + contains \ + represents \ + a \ + an \ + the +ALWAYS_DETAILED_SEC = NO +INLINE_INHERITED_MEMB = NO +FULL_PATH_NAMES = YES +STRIP_FROM_PATH = +STRIP_FROM_INC_PATH = +SHORT_NAMES = NO +JAVADOC_AUTOBRIEF = NO +JAVADOC_BANNER = NO +QT_AUTOBRIEF = NO +MULTILINE_CPP_IS_BRIEF = NO +INHERIT_DOCS = YES +SEPARATE_MEMBER_PAGES = NO +TAB_SIZE = 4 +ALIASES = +TCL_SUBST = +OPTIMIZE_OUTPUT_FOR_C = NO +OPTIMIZE_OUTPUT_JAVA = NO +OPTIMIZE_FOR_FORTRAN = NO +OPTIMIZE_OUTPUT_VHDL = NO +OPTIMIZE_OUTPUT_SLICE = NO +EXTENSION_MAPPING = +MARKDOWN_SUPPORT = YES +TOC_INCLUDE_HEADINGS = 5 +AUTOLINK_SUPPORT = YES +BUILTIN_STL_SUPPORT = NO +CPP_CLI_SUPPORT = NO +SIP_SUPPORT = NO +IDL_PROPERTY_SUPPORT = YES +DISTRIBUTE_GROUP_DOC = NO +GROUP_NESTED_COMPOUNDS = NO +SUBGROUPING = YES +INLINE_GROUPED_CLASSES = NO +INLINE_SIMPLE_STRUCTS = NO +TYPEDEF_HIDES_STRUCT = NO +LOOKUP_CACHE_SIZE = 0 +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- +EXTRACT_ALL = NO +EXTRACT_PRIVATE = YES +EXTRACT_PRIV_VIRTUAL = NO +EXTRACT_PACKAGE = NO +EXTRACT_STATIC = NO +EXTRACT_LOCAL_CLASSES = YES +EXTRACT_LOCAL_METHODS = NO +EXTRACT_ANON_NSPACES = NO +HIDE_UNDOC_MEMBERS = NO +HIDE_UNDOC_CLASSES = NO +HIDE_FRIEND_COMPOUNDS = NO +HIDE_IN_BODY_DOCS = NO +INTERNAL_DOCS = NO +CASE_SENSE_NAMES = YES +HIDE_SCOPE_NAMES = NO +HIDE_COMPOUND_REFERENCE= NO +SHOW_INCLUDE_FILES = YES +SHOW_GROUPED_MEMB_INC = NO +FORCE_LOCAL_INCLUDES = NO +INLINE_INFO = YES +SORT_MEMBER_DOCS = YES +SORT_BRIEF_DOCS = NO +SORT_MEMBERS_CTORS_1ST = NO +SORT_GROUP_NAMES = NO +SORT_BY_SCOPE_NAME = NO +STRICT_PROTO_MATCHING = NO +GENERATE_TODOLIST = YES +GENERATE_TESTLIST = YES +GENERATE_BUGLIST = YES +GENERATE_DEPRECATEDLIST= YES +ENABLED_SECTIONS = +MAX_INITIALIZER_LINES = 30 +SHOW_USED_FILES = YES +SHOW_FILES = YES +SHOW_NAMESPACES = YES +FILE_VERSION_FILTER = +LAYOUT_FILE = +CITE_BIB_FILES = +#--------------------------------------------------------------------------- +# Configuration options related to warning and progress messages +#--------------------------------------------------------------------------- +QUIET = YES +WARNINGS = YES +WARN_IF_UNDOCUMENTED = YES +WARN_IF_DOC_ERROR = YES +WARN_NO_PARAMDOC = NO +WARN_AS_ERROR = NO +WARN_FORMAT = "$file:$line: $text" +WARN_LOGFILE = +#--------------------------------------------------------------------------- +# Configuration options related to the input files +#--------------------------------------------------------------------------- +INPUT = src/ doc/ +INPUT_ENCODING = UTF-8 +FILE_PATTERNS = *.cpp \ + *.hpp \ + *.dox +RECURSIVE = YES +EXCLUDE = +EXCLUDE_SYMLINKS = NO +EXCLUDE_PATTERNS = +EXCLUDE_SYMBOLS = +EXAMPLE_PATH = +EXAMPLE_PATTERNS = * +EXAMPLE_RECURSIVE = NO +IMAGE_PATH = +INPUT_FILTER = +FILTER_PATTERNS = +FILTER_SOURCE_FILES = NO +FILTER_SOURCE_PATTERNS = +USE_MDFILE_AS_MAINPAGE = +#--------------------------------------------------------------------------- +# Configuration options related to source browsing +#--------------------------------------------------------------------------- +SOURCE_BROWSER = NO +INLINE_SOURCES = NO +STRIP_CODE_COMMENTS = YES +REFERENCED_BY_RELATION = NO +REFERENCES_RELATION = NO +REFERENCES_LINK_SOURCE = YES +SOURCE_TOOLTIPS = YES +USE_HTAGS = NO +VERBATIM_HEADERS = YES +#--------------------------------------------------------------------------- +# Configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- +ALPHABETICAL_INDEX = YES +COLS_IN_ALPHA_INDEX = 5 +IGNORE_PREFIX = +#--------------------------------------------------------------------------- +# Configuration options related to the HTML output +#--------------------------------------------------------------------------- +GENERATE_HTML = YES +HTML_OUTPUT = html +HTML_FILE_EXTENSION = .html +HTML_HEADER = +HTML_FOOTER = +HTML_STYLESHEET = +HTML_EXTRA_STYLESHEET = +HTML_EXTRA_FILES = +HTML_COLORSTYLE_HUE = 220 +HTML_COLORSTYLE_SAT = 100 +HTML_COLORSTYLE_GAMMA = 80 +HTML_TIMESTAMP = NO +HTML_DYNAMIC_MENUS = YES +HTML_DYNAMIC_SECTIONS = NO +HTML_INDEX_NUM_ENTRIES = 100 +GENERATE_DOCSET = NO +DOCSET_FEEDNAME = "Doxygen generated docs" +DOCSET_BUNDLE_ID = org.doxygen.Project +DOCSET_PUBLISHER_ID = org.doxygen.Publisher +DOCSET_PUBLISHER_NAME = Publisher +GENERATE_HTMLHELP = NO +CHM_FILE = +HHC_LOCATION = +GENERATE_CHI = NO +CHM_INDEX_ENCODING = +BINARY_TOC = NO +TOC_EXPAND = NO +GENERATE_QHP = NO +QCH_FILE = +QHP_NAMESPACE = org.doxygen.Project +QHP_VIRTUAL_FOLDER = doc +QHP_CUST_FILTER_NAME = +QHP_CUST_FILTER_ATTRS = +QHP_SECT_FILTER_ATTRS = +QHG_LOCATION = +GENERATE_ECLIPSEHELP = NO +ECLIPSE_DOC_ID = org.doxygen.Project +DISABLE_INDEX = NO +GENERATE_TREEVIEW = YES +ENUM_VALUES_PER_LINE = 4 +TREEVIEW_WIDTH = 250 +EXT_LINKS_IN_WINDOW = NO +HTML_FORMULA_FORMAT = png +FORMULA_FONTSIZE = 10 +FORMULA_TRANSPARENT = YES +FORMULA_MACROFILE = +USE_MATHJAX = YES +MATHJAX_FORMAT = HTML-CSS +MATHJAX_RELPATH = https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.5/ +MATHJAX_EXTENSIONS = +MATHJAX_CODEFILE = +SEARCHENGINE = YES +SERVER_BASED_SEARCH = NO +EXTERNAL_SEARCH = NO +SEARCHENGINE_URL = +SEARCHDATA_FILE = searchdata.xml +EXTERNAL_SEARCH_ID = +EXTRA_SEARCH_MAPPINGS = +#--------------------------------------------------------------------------- +# Configuration options related to the LaTeX output +#--------------------------------------------------------------------------- +GENERATE_LATEX = NO +LATEX_OUTPUT = latex +LATEX_CMD_NAME = +MAKEINDEX_CMD_NAME = makeindex +LATEX_MAKEINDEX_CMD = makeindex +COMPACT_LATEX = NO +PAPER_TYPE = a4 +EXTRA_PACKAGES = +LATEX_HEADER = +LATEX_FOOTER = +LATEX_EXTRA_STYLESHEET = +LATEX_EXTRA_FILES = +PDF_HYPERLINKS = YES +USE_PDFLATEX = YES +LATEX_BATCHMODE = NO +LATEX_HIDE_INDICES = NO +LATEX_SOURCE_CODE = NO +LATEX_BIB_STYLE = plain +LATEX_TIMESTAMP = NO +LATEX_EMOJI_DIRECTORY = +#--------------------------------------------------------------------------- +# Configuration options related to the RTF output +#--------------------------------------------------------------------------- +GENERATE_RTF = NO +RTF_OUTPUT = rtf +COMPACT_RTF = NO +RTF_HYPERLINKS = NO +RTF_STYLESHEET_FILE = +RTF_EXTENSIONS_FILE = +RTF_SOURCE_CODE = NO +#--------------------------------------------------------------------------- +# Configuration options related to the man page output +#--------------------------------------------------------------------------- +GENERATE_MAN = NO +MAN_OUTPUT = man +MAN_EXTENSION = .3 +MAN_SUBDIR = +MAN_LINKS = NO +#--------------------------------------------------------------------------- +# Configuration options related to the XML output +#--------------------------------------------------------------------------- +GENERATE_XML = NO +XML_OUTPUT = xml +XML_PROGRAMLISTING = YES +XML_NS_MEMB_FILE_SCOPE = NO +#--------------------------------------------------------------------------- +# Configuration options related to the DOCBOOK output +#--------------------------------------------------------------------------- +GENERATE_DOCBOOK = NO +DOCBOOK_OUTPUT = docbook +DOCBOOK_PROGRAMLISTING = NO +#--------------------------------------------------------------------------- +# Configuration options for the AutoGen Definitions output +#--------------------------------------------------------------------------- +GENERATE_AUTOGEN_DEF = NO +#--------------------------------------------------------------------------- +# Configuration options related to the Perl module output +#--------------------------------------------------------------------------- +GENERATE_PERLMOD = NO +PERLMOD_LATEX = NO +PERLMOD_PRETTY = YES +PERLMOD_MAKEVAR_PREFIX = +#--------------------------------------------------------------------------- +# Configuration options related to the preprocessor +#--------------------------------------------------------------------------- +ENABLE_PREPROCESSING = YES +MACRO_EXPANSION = NO +EXPAND_ONLY_PREDEF = NO +SEARCH_INCLUDES = YES +INCLUDE_PATH = +INCLUDE_FILE_PATTERNS = +PREDEFINED = +EXPAND_AS_DEFINED = +SKIP_FUNCTION_MACROS = YES +#--------------------------------------------------------------------------- +# Configuration options related to external references +#--------------------------------------------------------------------------- +TAGFILES = +GENERATE_TAGFILE = +ALLEXTERNALS = NO +EXTERNAL_GROUPS = YES +EXTERNAL_PAGES = YES +#--------------------------------------------------------------------------- +# Configuration options related to the dot tool +#--------------------------------------------------------------------------- +CLASS_DIAGRAMS = YES +DIA_PATH = +HIDE_UNDOC_RELATIONS = YES +HAVE_DOT = NO +DOT_NUM_THREADS = 0 +DOT_FONTNAME = Helvetica +DOT_FONTSIZE = 10 +DOT_FONTPATH = +CLASS_GRAPH = YES +COLLABORATION_GRAPH = YES +GROUP_GRAPHS = YES +UML_LOOK = NO +UML_LIMIT_NUM_FIELDS = 10 +TEMPLATE_RELATIONS = NO +INCLUDE_GRAPH = YES +INCLUDED_BY_GRAPH = YES +CALL_GRAPH = NO +CALLER_GRAPH = NO +GRAPHICAL_HIERARCHY = YES +DIRECTORY_GRAPH = YES +DOT_IMAGE_FORMAT = png +INTERACTIVE_SVG = NO +DOT_PATH = +DOTFILE_DIRS = +MSCFILE_DIRS = +DIAFILE_DIRS = +PLANTUML_JAR_PATH = +PLANTUML_CFG_FILE = +PLANTUML_INCLUDE_PATH = +DOT_GRAPH_MAX_NODES = 50 +MAX_DOT_GRAPH_DEPTH = 0 +DOT_TRANSPARENT = NO +DOT_MULTI_TARGETS = NO +GENERATE_LEGEND = YES +DOT_CLEANUP = YES diff --git a/cpp/doc/main.dox b/cpp/doc/main.dox new file mode 100644 index 00000000..eeaeb25c --- /dev/null +++ b/cpp/doc/main.dox @@ -0,0 +1,14 @@ +namespace toppra { +/** + +\mainpage TOPP-RA - Time Optimal Path Parameterization by Reachability Analysis. + +The main classes are: +- GeometricPath: \copybrief GeometricPath +- LinearConstraint: \copybrief LinearConstraint +- Solver: \copybrief Solver +- PathParametrizationAlgorithm: \copybrief PathParametrizationAlgorithm + +*/ + +} diff --git a/cpp/src/CMakeLists.txt b/cpp/src/CMakeLists.txt new file mode 100644 index 00000000..0796666b --- /dev/null +++ b/cpp/src/CMakeLists.txt @@ -0,0 +1,81 @@ +include(GenerateExportHeader) + +set(SOURCE_FILES + toppra/constraint.cpp + toppra/constraint/linear_joint_velocity.cpp + toppra/constraint/linear_joint_acceleration.cpp + toppra/constraint/joint_torque.cpp + + toppra/solver.cpp + + toppra/geometric_path.cpp + toppra/geometric_path/piecewise_poly_path.cpp + + toppra/algorithm.cpp + toppra/algorithm/toppra.cpp + ) +if(BUILD_WITH_qpOASES) + list(APPEND SOURCE_FILES + toppra/solver/qpOASES-wrapper.cpp) +endif() +if(BUILD_WITH_GLPK) + list(APPEND SOURCE_FILES + toppra/solver/glpk-wrapper.cpp) +endif() + +add_library(toppra SHARED ${SOURCE_FILES}) +FILE(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/toppra) +generate_export_header(toppra + EXPORT_FILE_NAME toppra/export.hpp + #INCLUDE_GUARD_NAME TOPPRA_EXPORT_HPP # CMake >= 3.11 only + ) +target_include_directories(toppra PUBLIC + $ + $ + $ + ${qpOASES_INCLUDE_DIRS} + ${GLPK_INCLUDE_DIRS} + ) +target_link_libraries(toppra PUBLIC + ${qpOASES_LIBRARIES} + ${GLPK_LIBRARIES} + ) +if (TOPPRA_DEBUG_ON) + target_compile_definitions(toppra PRIVATE TOPPRA_DEBUG_ON) +endif() + +if (${EIGEN3_VERSION_STRING} VERSION_LESS 3.3) + target_compile_definitions (toppra PUBLIC ${EIGEN3_DEFINITIONS} ) + target_include_directories (toppra PUBLIC ${EIGEN3_INCLUDE_DIRS} ) +else (${EIGEN3_VERSION_STRING} VERSION_LESS 3.3) + target_link_libraries(toppra PUBLIC Eigen3::Eigen) +endif (${EIGEN3_VERSION_STRING} VERSION_LESS 3.3) + +install(TARGETS toppra EXPORT toppra::toppra + DESTINATION lib) +install(FILES + toppra/algorithm.hpp + toppra/constraint.hpp + ${CMAKE_CURRENT_BINARY_DIR}/toppra/export.hpp + + toppra/geometric_path.hpp + toppra/toppra.hpp + + toppra/solver.hpp + + DESTINATION include/toppra) +install(FILES + toppra/algorithm/toppra.hpp + DESTINATION include/toppra/algorithm) +install(FILES + toppra/constraint/linear_joint_velocity.hpp + toppra/constraint/linear_joint_acceleration.hpp + toppra/constraint/joint_torque.hpp + DESTINATION include/toppra/constraint) +install(FILES + toppra/constraint/joint_torque/pinocchio.hpp + DESTINATION include/toppra/constraint/joint_torque) +install(FILES + toppra/solver/qpOASES-wrapper.hpp + toppra/solver/glpk-wrapper.hpp + DESTINATION include/toppra/solver) diff --git a/cpp/src/toppra/algorithm.cpp b/cpp/src/toppra/algorithm.cpp new file mode 100644 index 00000000..a1a89d50 --- /dev/null +++ b/cpp/src/toppra/algorithm.cpp @@ -0,0 +1,118 @@ +#include +#include +#include +#include +#include "toppra/toppra.hpp" + +namespace toppra { + +PathParametrizationAlgorithm::PathParametrizationAlgorithm( + LinearConstraintPtrs constraints, const GeometricPath &path) + : m_constraints(std::move(constraints)), m_path(path){}; + +ReturnCode PathParametrizationAlgorithm::computePathParametrization(value_type vel_start, + value_type vel_end) { + ReturnCode ret; + initialize(); + m_solver->setupSolver(); + Bound vel_ends; + vel_ends.setConstant(vel_end); + ret = computeControllableSets(vel_ends); + if ((int)ret > 0) { + return ret; + } + ret = computeForwardPass(vel_start); + return ret; +}; + +ReturnCode PathParametrizationAlgorithm::computeControllableSets( + const Bound &vel_ends) { + TOPPRA_LOG_DEBUG("computeControllableSets"); + ReturnCode ret = ReturnCode::OK; + bool solver_ret; + Vector g_upper{2}, g_lower{2}, solution; + g_upper << 1e-9, -1; + g_lower << -1e-9, 1; + m_data.controllable_sets.row(m_N) = vel_ends.cwiseAbs2(); + + Matrix H; + Bound x, x_next; + x << 0, 100; + x_next << 0, 1; + for (std::size_t i = m_N - 1; i != (std::size_t)-1; i--) { + TOPPRA_LOG_DEBUG(i << ", " << m_N); + x_next << m_data.controllable_sets.row(i + 1); + solver_ret = m_solver->solveStagewiseOptim(i, H, g_upper, x, x_next, solution); + + if (!solver_ret) { + ret = ReturnCode::ERR_FAIL_CONTROLLABLE; + TOPPRA_LOG_DEBUG("Fail: controllable, upper problem, idx: " << i); + break; + } + + m_data.controllable_sets(i, 1) = solution[1]; + + solver_ret = m_solver->solveStagewiseOptim(i, H, g_lower, x, x_next, solution); + + TOPPRA_LOG_DEBUG("down: " << solution); + + if (!solver_ret) { + ret = ReturnCode::ERR_FAIL_CONTROLLABLE; + TOPPRA_LOG_DEBUG("Fail: controllable, lower problem, idx: " << i); + break; + } + + m_data.controllable_sets(i, 0) = solution[1]; + } + return ret; +} + +ReturnCode PathParametrizationAlgorithm::computeFeasibleSets() { + initialize(); + ReturnCode ret = ReturnCode::OK; + bool solver_ret; + Vector g_upper{2}, g_lower{2}, solution; + g_upper << 1e-9, -1; + g_lower << -1e-9, 1; + + Matrix H; + Bound x, x_next; + x << 0, 100; + x_next << 0, 100; + for (std::size_t i = 0; i < m_N + 1; i++) { + solver_ret = m_solver->solveStagewiseOptim(i, H, g_upper, x, x_next, solution); + + if (!solver_ret) { + ret = ReturnCode::ERR_FAIL_FEASIBLE; + TOPPRA_LOG_DEBUG("Fail: controllable, upper problem, idx: " << i); + break; + } + + m_data.feasible_sets(i, 1) = solution[1]; + + solver_ret = m_solver->solveStagewiseOptim(i, H, g_lower, x, x_next, solution); + + if (!solver_ret) { + ret = ReturnCode::ERR_FAIL_FEASIBLE; + TOPPRA_LOG_DEBUG("Fail: controllable, lower problem, idx: " << i); + break; + } + + m_data.feasible_sets(i, 0) = solution[1]; + } + return ret; +} + +void PathParametrizationAlgorithm::initialize() { + if (m_initialized) return; + m_data.gridpoints = + Vector::LinSpaced(m_N + 1, m_path.pathInterval()(0), m_path.pathInterval()(1)); + m_data.parametrization.resize(m_N + 1); + m_data.controllable_sets.resize(m_N + 1, 2); + m_data.feasible_sets.resize(m_N + 1, 2); + m_solver = std::make_shared(m_constraints, m_path, + m_data.gridpoints); + m_initialized = true; +} + +} // namespace toppra diff --git a/cpp/src/toppra/algorithm.hpp b/cpp/src/toppra/algorithm.hpp new file mode 100644 index 00000000..fbc594a1 --- /dev/null +++ b/cpp/src/toppra/algorithm.hpp @@ -0,0 +1,123 @@ +#ifndef TOPPRA_ALGORITHM_HPP +#define TOPPRA_ALGORITHM_HPP + +#include +#include +#include +#include +#include + +namespace toppra { + +/// Return code for Path Parametrization algorithm. +enum class ReturnCode { + + /// Success + OK = 0, + + /// Unknown error + ERR_UNKNOWN = 1, + + /// Fail during computing controllable sets. Problem might be infeasible. + ERR_FAIL_CONTROLLABLE = 2, + + /// Fail during forward pass. Numerical error occured. + ERR_FAIL_FORWARD_PASS = 3, + + /// Problem is not initialized + ERR_UNINITIALIZED = 4, + + /// Fail to ocmpute feasible sets. + ERR_FAIL_FEASIBLE = 5, +}; + +struct ParametrizationData { + /// \brief Grid-points used for solving the discretized problem. + /// The number of points must equal m_N + 1. + Vector gridpoints; + + /// Output parametrization (squared path velocity) + Vector parametrization; + + Matrix controllable_sets; + Matrix feasible_sets; + + /// Return code of the algorithm. + ReturnCode ret_code = ReturnCode::ERR_UNINITIALIZED; +}; + +/** \brief Base class for time parametrization algorithms. + * + */ +class PathParametrizationAlgorithm { + public: + /** Construct the problem instance. + * + * \param constraints List of constraints. + * \param path The geometric path. + * + */ + PathParametrizationAlgorithm(LinearConstraintPtrs constraints, + const GeometricPath &path); + + /** \brief Set the level of discretization used by the solver. + * + * If is zero, will attempt to detect automatically the most suitable grid. + */ + void setN(int N) { m_N = N; }; + + /** \brief Get output or result of algorithm. + */ + ParametrizationData getParameterizationData() const { return m_data; }; + + /** Compute the time parametrization of the given path. + * + * \param vel_start + * \param vel_end + * \return Return code. + */ + virtual ReturnCode computePathParametrization(value_type vel_start = 0, + value_type vel_end = 0); + + /** Compute the sets of feasible squared velocities. + */ + ReturnCode computeFeasibleSets(); + + virtual ~PathParametrizationAlgorithm() {} + + protected: + /** \brief Select solver and gridpoints to use. + * + * This method implements a simple way to select gridpoints. + */ + virtual void initialize(); + + /** \brief Compute the forward pass. + * + * Derived class should provide a suitable forward pass function, + * depending on the desired objective. + */ + virtual ReturnCode computeForwardPass(value_type vel_start) = 0; + + /** Compute the sets of controllable squared path velocities. + */ + ReturnCode computeControllableSets(const Bound &vel_ends); + + /** To be implemented in child method. */ + LinearConstraintPtrs m_constraints; + const GeometricPath &m_path; + SolverPtr m_solver; + + /// Struct containing algorithm output. + ParametrizationData m_data; + + /// \brief Number of segments in the discretized problems. + /// See m_gridpoints for more information. + int m_N = 100; + + int m_initialized = false; +}; + +} // namespace toppra + +#endif diff --git a/cpp/src/toppra/algorithm/toppra.cpp b/cpp/src/toppra/algorithm/toppra.cpp new file mode 100644 index 00000000..17092655 --- /dev/null +++ b/cpp/src/toppra/algorithm/toppra.cpp @@ -0,0 +1,40 @@ +#include +#include +#include +#include + +namespace toppra { +namespace algorithm { + +TOPPRA::TOPPRA(LinearConstraintPtrs constraints, const GeometricPath &path) + : PathParametrizationAlgorithm{std::move(constraints), path} {}; + +ReturnCode TOPPRA::computeForwardPass(value_type vel_start) { + TOPPRA_LOG_DEBUG("computeForwardPass"); + ReturnCode ret = ReturnCode::OK; + bool solver_ret; + Vector g_upper{2}, solution; + Matrix H; + auto deltas = m_solver->deltas(); + Bound x, x_next; + m_data.parametrization(0) = vel_start; + for (std::size_t i = 0; i < m_N; i++) { + g_upper << -2 * deltas(i), -1; + x.setConstant(m_data.parametrization(i)); + x_next = m_data.controllable_sets.row(i + 1); + solver_ret = m_solver->solveStagewiseOptim(i, H, g_upper, x, x_next, solution); + if (!solver_ret) { + ret = ReturnCode::ERR_FAIL_FORWARD_PASS; + TOPPRA_LOG_DEBUG("Fail: forward pass, idx: " << i); + break; + } + /// \todo This can be optimized further by solving a 1D problem instead of 2D + m_data.parametrization(i + 1) = + m_data.parametrization(i) + 2 * deltas(i) * solution(0); + } + + return ret; +}; + +} // namespace algorithm +} // namespace toppra diff --git a/cpp/src/toppra/algorithm/toppra.hpp b/cpp/src/toppra/algorithm/toppra.hpp new file mode 100644 index 00000000..3f34bce8 --- /dev/null +++ b/cpp/src/toppra/algorithm/toppra.hpp @@ -0,0 +1,21 @@ +#ifndef TOPPRA_ALGORITHM_TOPPRA_HPP +#define TOPPRA_ALGORITHM_TOPPRA_HPP + +#include +#include +#include +#include "toppra/toppra.hpp" + +namespace toppra { +namespace algorithm { +class TOPPRA : public PathParametrizationAlgorithm { + public: + TOPPRA(LinearConstraintPtrs constraints, const GeometricPath &path); + + protected: + ReturnCode computeForwardPass(value_type vel_start); +}; +} // namespace algorithm +} // namespace toppra + +#endif diff --git a/cpp/src/toppra/constraint.cpp b/cpp/src/toppra/constraint.cpp new file mode 100644 index 00000000..c0822be7 --- /dev/null +++ b/cpp/src/toppra/constraint.cpp @@ -0,0 +1,206 @@ +#include + +#include + +namespace toppra { + +std::ostream& LinearConstraint::print(std::ostream& os) const +{ + return os << + " Discretization Scheme: " << m_discretizationType << "\n" + " Has " << (hasLinearInequalities() ? "linear inequalities, " : "") + << (hasUbounds() ? "bounds on u, " : "") + << (hasXbounds() ? "bounds on x, " : "") << "\n"; +} + +void LinearConstraint::discretizationType (DiscretizationType type) +{ + // I don't think the check done in Python is useful in C++. + m_discretizationType = type; +} + +/// \internal + +/// \param k number of constraints +/// \param m number of variables +void allocateLinearPart(std::size_t N, Eigen::Index k, Eigen::Index m, + bool constantF, Vectors& a, Vectors& b, Vectors& c, Matrices& F, Vectors& g) + TOPPRA_NO_EXPORT; + +void allocateLinearPart(std::size_t N, Eigen::Index k, Eigen::Index m, + bool constantF, Vectors& a, Vectors& b, Vectors& c, Matrices& F, Vectors& g) +{ + a.resize(N); + b.resize(N); + c.resize(N); + if (constantF) { + F.resize(1); + g.resize(1); + } else { + F.resize(N); + g.resize(N); + } + for (Vector& x : a) x.resize(m); + for (Vector& x : b) x.resize(m); + for (Vector& x : c) x.resize(m); + for (Matrix& x : F) x.resize(k, m); + for (Vector& x : g) x.resize(k); +} + +void checkSizes (std::size_t N, Eigen::Index k, Eigen::Index m, + bool constantF, Vectors& a, Vectors& b, Vectors& c, Matrices& F, Vectors& g) + TOPPRA_NO_EXPORT; + +void checkSizes (std::size_t N, Eigen::Index k, Eigen::Index m, + bool constantF, Vectors& a, Vectors& b, Vectors& c, Matrices& F, Vectors& g) +{ + if (a.size() != N) + throw std::invalid_argument("Wrong number of a vectors"); + if (b.size() != N) + throw std::invalid_argument("Wrong number of b vectors"); + if (c.size() != N) + throw std::invalid_argument("Wrong number of c vectors"); + if (constantF && F.size() != 1) + throw std::invalid_argument("Expected only one F matrix"); + if (constantF && g.size() != 1) + throw std::invalid_argument("Expected only one g matrix"); + if (!constantF && g.size() != N) + throw std::invalid_argument("Wrong number of g matrices"); + + for (std::size_t i = 0; i < N; ++i) { + if (a[i].size() != m) + throw std::invalid_argument("Wrong a[i] vector size."); + if (b[i].size() != m) + throw std::invalid_argument("Wrong b[i] vector size."); + if (c[i].size() != m) + throw std::invalid_argument("Wrong c[i] vector size."); + if (constantF) { + if (i == 0 && (F[0].rows() != k || F[0].cols() != m)) + throw std::invalid_argument("Wrong F[0] matrix dimensions."); + if (i == 0 && g[0].size() != k) + throw std::invalid_argument("Wrong g[0] vector size."); + } else { + if (F[i].rows() != k || F[i].cols() != m) + throw std::invalid_argument("Wrong F[i] matrix dimensions."); + if (g[i].size() != k) + throw std::invalid_argument("Wrong g[i] vector size."); + } + } +} + +/// Convert from Collocation to Interpolation +void collocationToInterpolate (const Vector& gridpoints, + bool constantF, + const Vectors& a_col, const Vectors& b_col, const Vectors& c_col, + const Matrices& F_col, const Vectors& g_col, + Vectors& a_intp, Vectors& b_intp, Vectors& c_intp, + Matrices& F_intp, Vectors& g_intp) + TOPPRA_NO_EXPORT; + +void collocationToInterpolate (const Vector& gridpoints, + bool constantF, + const Vectors& a_col, const Vectors& b_col, const Vectors& c_col, + const Matrices& F_col, const Vectors& g_col, + Vectors& a_intp, Vectors& b_intp, Vectors& c_intp, + Matrices& F_intp, Vectors& g_intp) +{ + std::size_t N (gridpoints.size()-1); + Vector deltas (gridpoints.tail(N) - gridpoints.head(N)); + Eigen::Index m (a_col[0].size()), + k (g_col[0].size()); + + //a_intp[:, :d] = a + //a_intp[:-1, d:] = a[1:] + 2 * deltas.reshape(-1, 1) * b[1:] + //a_intp[-1, d:] = a_intp[-1, :d] + for (std::size_t i = 0; i <= N; ++i) { + a_intp[i].head(m) = a_col[i]; + if (i < N) + a_intp[i].tail(m) = a_col[i+1] + 2 * deltas.cwiseProduct(b_col[i+1].tail(N)); + else + a_intp[N].tail(m) = a_col[N]; + } + + // b_intp[:, :d] = b + // b_intp[:-1, d:] = b[1:] + // b_intp[-1, d:] = b_intp[-1, :d] + for (std::size_t i = 0; i <= N; ++i) { + b_intp[i].head(m) = b_col[i]; + b_intp[i].tail(m) = b_col[std::min(i+1, N)]; + } + + // c_intp[:, :d] = c + // c_intp[:-1, d:] = c[1:] + // c_intp[-1, d:] = c_intp[-1, :d] + for (std::size_t i = 0; i <= N; ++i) { + c_intp[i].head(m) = c_col[i]; + c_intp[i].tail(m) = c_col[std::min(i+1, N)]; + } + + const auto zero (Matrix::Zero (2 * k, 2 * m)); + if (constantF) { + g_intp[0] << g_col[0], g_col[0]; + + F_intp[0] << F_col[0], zero, + zero, F_col[0]; + } else { + // g_intp[:, :m] = g + // g_intp[:-1, m:] = g[1:] + // g_intp[-1, m:] = g_intp[-1, :m] + for (std::size_t i = 0; i <= N; ++i) { + g_intp[i].head(k) = g_col[i]; + g_intp[i].tail(k) = g_col[std::min(i+1,N)]; + } + + // F_intp[:, :m, :d] = F + // F_intp[:-1, m:, d:] = F[1:] + // F_intp[-1, m:, d:] = F[-1] + for (std::size_t i = 0; i <= N; ++i) { + F_intp[i] << F_col[i], zero, + zero, F_col[std::min(i+1,N)]; + } + } +} + +/// \endinternal + +void LinearConstraint::allocateParams(std::size_t N, + Vectors& a, Vectors& b, Vectors& c, Matrices& F, Vectors& g, + Bounds ubound, Bounds& xbound) +{ + if (hasLinearInequalities()) { + Eigen::Index m (nbVariables()), k (nbConstraints()); + if (m_discretizationType == Interpolation) { + m *= 2; + k *= 2; + } + + allocateLinearPart (N, k, m, constantF(), a, b, c, F, g); + } + if (hasUbounds()) + ubound.resize(N); + if (hasXbounds()) + xbound.resize(N); +} + +void LinearConstraint::computeParams(const GeometricPath& path, const Vector& gridpoints, + Vectors& a, Vectors& b, Vectors& c, Matrices& F, Vectors& g, + Bounds& ubound, Bounds& xbound) +{ + Eigen::Index N = gridpoints.size(); + assert (N > 0); + allocateParams(gridpoints.size(), a, b, c, F, g, ubound, xbound); + + if (m_discretizationType == Interpolation && hasLinearInequalities()) { + Vectors a_col, b_col, c_col, g_col; + Matrices F_col; + allocateLinearPart (N, m_k, m_m, constantF(), a, b, c, F, g); + computeParams_impl(path, gridpoints, a_col, b_col, c_col, F_col, g_col, ubound, xbound); + collocationToInterpolate(gridpoints, constantF(), + a_col, b_col, c_col, F_col, g_col, + a, b, c, F, g); + } else { + computeParams_impl(path, gridpoints, a, b, c, F, g, ubound, xbound); + } +} + +} // namespace toppra diff --git a/cpp/src/toppra/constraint.hpp b/cpp/src/toppra/constraint.hpp new file mode 100644 index 00000000..8a522722 --- /dev/null +++ b/cpp/src/toppra/constraint.hpp @@ -0,0 +1,168 @@ +#ifndef TOPPRA_CONSTRAINT_HPP +#define TOPPRA_CONSTRAINT_HPP + +#include +#include + +namespace toppra { +/** Enum to mark different Discretization Scheme for LinearConstraint. + * In general, the difference in speed is not too large. Should use + * \ref Interpolation if possible. + * */ +enum DiscretizationType { + Collocation, /// smaller problem size, but lower accuracy. + Interpolation, /// larger problem size, but higher accuracy. +}; + +/** \brief Abstract interface for the constraints. + * + * Also known as Second-order Constraint. + * + * A Canonical Linear Constraint has the following form: + * \f{eqnarray} + * \mathbf a_i u + \mathbf b_i x + \mathbf c_i &= v \\ + * \mathbf F_i v & \leq \mathbf g_i \\ + * x^b_{i, 0} \leq x & \leq x^b_{i, 1} \\ + * u^b_{i, 0} \leq u & \leq u^b_{i, 1} + * \f} + * + * Alternatively, if \f$ \mathbf F_i \f$ is constant for all values + * of \f$i\f$, then we can consider the simpler constraint: + * \f[ + * \mathbf{F} v \leq \mathbf g + * \f] + * + * In this case, the returned value of \f$F\f$ by + * LinearConstraint::computeParams has shape (k, m) instead of (N, k, m), + * \f$ g \f$ (k) instead of (N, k) and the class attribute + * LinearConstraint::constantF will be \c true. + * + * \note Derived classes should at least implement the method + * LinearConstraint::computeParams_impl. + * + * \sa JointAccelerationConstraint, JointVelocityConstraint, + * CanonicalLinearSecondOrderConstraint + * + * */ +class LinearConstraint { + public: + DiscretizationType discretizationType () const + { + return m_discretizationType; + } + + void discretizationType (DiscretizationType type); + + /** Tells whether \f$ F, g \f$ matrices are the same over all the grid points. + * In this case, LinearConstraint::computeParams F and g parameters should + * only be of size 1. + * */ + bool constantF () const + { + return m_constantF; + } + + /// Dimension of \f$g\f$. + Eigen::Index nbConstraints () const + { + return m_k; + } + + /// Dimension of \f$a, b, c, v\f$. + Eigen::Index nbVariables () const + { + return m_m; + } + + bool hasLinearInequalities () const + { + return nbConstraints() > 0; + } + + /** Whether this constraint has bounds on \f$u\f$. + * */ + bool hasUbounds () const + { + return m_hasUbounds; + } + + /** Whether this constraint has bounds on \f$x\f$. + * */ + bool hasXbounds () const + { + return m_hasXbounds; + } + + /** + * \param N number of gripoints (i.e. the number of intervals + 1) + * */ + void allocateParams (std::size_t N, + Vectors& a, Vectors& b, Vectors& c, + Matrices& F, Vectors& g, + Bounds ubound, Bounds& xbound); + + /** Compute numerical coefficients of the given constraint. + * + * \param[in] path The geometric path. + * \param[in] gridpoints Vector of size N+1. Gridpoint use for discretizing path. + * + * \param[out] a N+1 Vector of size m. + * \param[out] b N+1 Vector of size m. + * \param[out] c N+1 Vector of size m. + * \param[out] F N+1 Matrix of shape (k, m). If LinearConstraint::constantF + * is \c true, there is only one such Matrix. + * \param[out] g N+1 Vector of size m. + * \param[out] ubound Shape (N + 1, 2). See notes. + * \param[out] xbound Shape (N + 1, 2). See notes. + * + * \note the output must be allocated to correct sizes prior to calling this + * function. + * + * \todo check constness + * + * */ + void computeParams(const GeometricPath& path, const Vector& gridpoints, + Vectors& a, Vectors& b, Vectors& c, + Matrices& F, Vectors& g, + Bounds& ubound, Bounds& xbound); + + virtual std::ostream& print(std::ostream& os) const; + + virtual ~LinearConstraint () {} + + protected: + /** + * \param k number of inequality constraints. + * \param m number of internal variable (i.e. dimention of \f$v\f$). + * \param constantF whether \f$F\f$ and \f$g\f$ are constant. + * \param uBound whether \f$u\f$ is bounded. + * \param xBound whether \f$x\f$ is bounded. + * */ + LinearConstraint(Eigen::Index k, Eigen::Index m, bool constantF, + bool uBound, bool xBound) + : m_discretizationType (Collocation) + , m_k (k), m_m (m) + , m_constantF (constantF) + , m_hasUbounds (uBound) + , m_hasXbounds (xBound) + {} + + virtual void computeParams_impl(const GeometricPath& path, + const Vector& gridpoints, + Vectors& a, Vectors& b, Vectors& c, + Matrices& F, Vectors& g, + Bounds& ubound, Bounds& xbound) = 0; + + Eigen::Index m_k, m_m; + DiscretizationType m_discretizationType; + bool m_constantF, m_hasUbounds, m_hasXbounds; +}; // class LinearConstraint + +inline std::ostream& operator<< (std::ostream& os, const LinearConstraint& lc) +{ + return lc.print(os); +} + +} // namespace toppra + +#endif diff --git a/cpp/src/toppra/constraint/joint_torque.cpp b/cpp/src/toppra/constraint/joint_torque.cpp new file mode 100644 index 00000000..9fbed6f0 --- /dev/null +++ b/cpp/src/toppra/constraint/joint_torque.cpp @@ -0,0 +1,60 @@ +#include + +#include + +namespace toppra { +namespace constraint { + +std::ostream& JointTorque::print (std::ostream& os) const +{ + os << "JointTorque\n"; + return LinearConstraint::print(os) << + " Lower torque limit: " << m_lower.transpose() << "\n" + " Upper torque limit: " << m_upper.transpose() << "\n"; +} + +void JointTorque::check () +{ + if (m_lower.size() != m_upper.size()) + throw std::invalid_argument("Torque limits size must match."); + if (m_lower.size() != m_frictionCoeffs.size()) + throw std::invalid_argument("Torque limits size and friction vector size must match."); + if ((m_lower.array() > m_upper.array()).any()) + throw std::invalid_argument("Bad torque limits."); +} + +void JointTorque::computeParams_impl(const GeometricPath& path, + const Vector& times, + Vectors& a, Vectors& b, Vectors& c, Matrices& F, Vectors& g, + Bounds&, Bounds&) +{ + Eigen::Index N = times.size(); + Eigen::Index ndofs (m_lower.size()); + + // Compute static F and g + F[0].topRows(ndofs).setIdentity(); + F[0].bottomRows(ndofs).setZero(); + F[0].bottomRows(ndofs).diagonal().setConstant(-1); + g[0].head(ndofs) = m_upper; + g[0].tail(ndofs) = - m_lower; + + Vector zero = Vector::Zero(ndofs); + for (std::size_t i = 0; i < N; ++i) { + Vector cfg = path.eval_single(times[i], 0); + Vector vel = path.eval_single(times[i], 1); + Vector acc = path.eval_single(times[i], 2); + assert(vel.size() == ndofs); + assert(acc.size() == ndofs); + + computeInverseDynamics(cfg, zero, zero, c[i]); + computeInverseDynamics(cfg, zero, vel, a[i]); + a[i] -= c[i]; + computeInverseDynamics(cfg, vel, acc, b[i]); + b[i] -= c[i]; + + c[i].array() += vel.array().sign() * m_frictionCoeffs.array(); + } +} + +} // namespace constraint +} // namespace toppra diff --git a/cpp/src/toppra/constraint/joint_torque.hpp b/cpp/src/toppra/constraint/joint_torque.hpp new file mode 100644 index 00000000..463ebae7 --- /dev/null +++ b/cpp/src/toppra/constraint/joint_torque.hpp @@ -0,0 +1,77 @@ +#ifndef TOPPRA_CONSTRAINT_JOINT_TORQUE_HPP +#define TOPPRA_CONSTRAINT_JOINT_TORQUE_HPP + +#include + +namespace toppra { +namespace constraint { + +/** Base class for joint torque constraints. + * + * A joint torque constraint is given by + * \f[ + * A(q) \ddot q + \dot q^\top B(q) \dot q + C(q) + D( \dot q )= \tau + * \f] + * + * where w is a vector that satisfies the polyhedral constraint: + * \f[ F \tau \leq g \f] + * + * Notice that \f$ inverseDynamics(q, qd, qdd) = \tau\f$ and that + * \f$ F, g \f$ are independant of the GeometricPath. + * + * To evaluate the constraint on a geometric path `p(s)`, multiple + * calls to \ref computeInverseDynamics are made. Specifically one + * can derive the second-order equation as follows + * + * \f{eqnarray} + * A(q) p'(s) \ddot s &+ [A(q) p''(s) + p'(s)^\top B(q) p'(s)] \dot s^2 &+ C(q) + D( \dot q ) &= \tau \\ + * a(s) \ddot s &+ b(s) \dot s^2 &+ c(s) &= \tau + * \f} + * + * */ +class JointTorque : public LinearConstraint { + public: + virtual ~JointTorque () {} + + virtual std::ostream& print(std::ostream& os) const; + + /** Computes the joint torques from + * \param q robot configuration + * \param v robot velocity + * \param a robot acceleration + * \param[out] joint torques + * */ + virtual void computeInverseDynamics (const Vector& q, const Vector& v, const Vector& a, + Vector& tau) = 0; + + protected: + /** + * \param lowerTlimit lower torque limit + * \param upperTlimit upper torque limit + * \param frictionCoeffs dry friction coefficients of each joint. + * */ + JointTorque (const Vector& lowerTlimit, const Vector& upperTlimit, + const Vector& frictionCoeffs) + : LinearConstraint (2*lowerTlimit.size(), lowerTlimit.size(), true, false, false) + , m_lower (lowerTlimit) + , m_upper (upperTlimit) + , m_frictionCoeffs (frictionCoeffs) + { + check(); + } + + private: + void check(); + + void computeParams_impl(const GeometricPath& path, + const Vector& gridpoints, + Vectors& a, Vectors& b, Vectors& c, + Matrices& F, Vectors& g, + Bounds& ubound, Bounds& xbound); + + Vector m_lower, m_upper, m_frictionCoeffs; +}; // class JointTorque +} // namespace constraint +} // namespace toppra + +#endif diff --git a/cpp/src/toppra/constraint/joint_torque/pinocchio.hpp b/cpp/src/toppra/constraint/joint_torque/pinocchio.hpp new file mode 100644 index 00000000..a7c8361c --- /dev/null +++ b/cpp/src/toppra/constraint/joint_torque/pinocchio.hpp @@ -0,0 +1,52 @@ +#ifndef TOPPRA_CONSTRAINT_JOINT_TORQUE_PINOCCIO_HPP +#define TOPPRA_CONSTRAINT_JOINT_TORQUE_PINOCCIO_HPP + +#include +#include +#include + +#include + +namespace toppra { +namespace constraint { +namespace jointTorque { + +template +class Pinocchio; + +/** Implementation of JointTorque using pinocchio::rnea function. + * */ +template +class Pinocchio : public JointTorque { + public: + typedef _Model Model; + typedef typename _Model::Data Data; + + std::ostream& print(std::ostream& os) const + { + return JointTorque::print(os << "Pinocchio - "); + } + + void computeInverseDynamics (const Vector& q, const Vector& v, const Vector& a, + Vector& tau) + { + tau = pinocchio::rnea(m_model, m_data, q, v, a); + } + + Pinocchio (const Model& model, const Vector& frictionCoeffs) + : JointTorque (-model.effortLimit, model.effortLimit, frictionCoeffs) + , m_model (model) + , m_data (model) + { + } + + private: + const Model& m_model; + Data m_data; +}; // class Pinocchio + +} // namespace jointTorque +} // namespace constraint +} // namespace toppra + +#endif diff --git a/cpp/src/toppra/constraint/linear_joint_acceleration.cpp b/cpp/src/toppra/constraint/linear_joint_acceleration.cpp new file mode 100644 index 00000000..0435cfbe --- /dev/null +++ b/cpp/src/toppra/constraint/linear_joint_acceleration.cpp @@ -0,0 +1,52 @@ +#include + +#include + +namespace toppra { +namespace constraint { + +std::ostream& LinearJointAcceleration::print (std::ostream& os) const +{ + os << "LinearJointAcceleration\n"; + return LinearConstraint::print(os) << + " Lower acceleration limit: " << m_lower.transpose() << "\n" + " Upper acceleration limit: " << m_upper.transpose() << "\n"; +} + +void LinearJointAcceleration::check () +{ + if ((m_lower.array() > m_upper.array()).any()) + throw std::invalid_argument("Bad acceleration limits."); +} + +void LinearJointAcceleration::computeParams_impl(const GeometricPath& path, + const Vector& times, + Vectors& a, Vectors& b, Vectors& c, + Matrices& F, Vectors& g, + Bounds&, Bounds&) +{ + Eigen::Index N_1 = times.size(); + + Eigen::Index ndofs (nbVariables()); + + // Compute F and g + Matrix& _F = F[0]; + _F. topRows(ndofs).setIdentity(); + _F.bottomRows(ndofs).setZero(); + _F.bottomRows(ndofs).diagonal().setConstant(-1); + Vector& _g = g[0]; + _g.head(ndofs) = m_upper; + _g.tail(ndofs) = -m_lower; + + assert(ndofs == path.dof()); + for (std::size_t i = 0; i < N_1; ++i) { + a[i] = path.eval_single(times[i], 1); + assert(a[i].size() == ndofs); + b[i] = path.eval_single(times[i], 2); + assert(b[i].size() == ndofs); + c[i].setZero(); + } +} + +} // namespace constraint +} // namespace toppra diff --git a/cpp/src/toppra/constraint/linear_joint_acceleration.hpp b/cpp/src/toppra/constraint/linear_joint_acceleration.hpp new file mode 100644 index 00000000..2da8fda3 --- /dev/null +++ b/cpp/src/toppra/constraint/linear_joint_acceleration.hpp @@ -0,0 +1,36 @@ +#ifndef TOPPRA_CONSTRAINT_LINEAR_JOINT_ACCELERATION_HPP +#define TOPPRA_CONSTRAINT_LINEAR_JOINT_ACCELERATION_HPP + +#include + +namespace toppra { +namespace constraint { + +/// A Joint Acceleration Constraint class. +class LinearJointAcceleration : public LinearConstraint { + public: + LinearJointAcceleration (const Vector& lowerAlimit, const Vector& upperAlimit) + : LinearConstraint (lowerAlimit.size() * 2, lowerAlimit.size(), true, false, false) + , m_lower (lowerAlimit) + , m_upper (upperAlimit) + { + check(); + } + + virtual std::ostream& print(std::ostream& os) const; + + private: + void check(); + + void computeParams_impl(const GeometricPath& path, + const Vector& gridpoints, + Vectors& a, Vectors& b, Vectors& c, + Matrices& F, Vectors& g, + Bounds& ubound, Bounds& xbound); + + Vector m_lower, m_upper; +}; // class LinearJointAcceleration +} // namespace constraint +} // namespace toppra + +#endif diff --git a/cpp/src/toppra/constraint/linear_joint_velocity.cpp b/cpp/src/toppra/constraint/linear_joint_velocity.cpp new file mode 100644 index 00000000..38d5265f --- /dev/null +++ b/cpp/src/toppra/constraint/linear_joint_velocity.cpp @@ -0,0 +1,62 @@ +#include + +#include + +namespace toppra { +namespace constraint { + +std::ostream& LinearJointVelocity::print (std::ostream& os) const +{ + os << "LinearJointVelocity\n"; + return LinearConstraint::print(os) << + " Lower velocity limit: " << m_lower.transpose() << "\n" + " Upper velocity limit: " << m_upper.transpose() << "\n"; +} + +void LinearJointVelocity::check () +{ + if (m_lower.size() != m_upper.size()) + throw std::invalid_argument("Velocity limits size must match."); + if ((m_lower.array() > m_upper.array()).any()) + throw std::invalid_argument("Bad velocity limits."); +} + +void LinearJointVelocity::computeParams_impl(const GeometricPath& path, + const Vector& gridpoints, + Vectors&, Vectors&, Vectors&, Matrices&, Vectors&, + Bounds&, Bounds& xbound) +{ + Eigen::Index N_1 = gridpoints.size(); + Eigen::Index ndofs (m_lower.size()); + assert(path.dof() == m_lower.size()); + + Vector v, v_inv(ndofs), lb_v(ndofs); + for (std::size_t i = 0; i < N_1; ++i) { + v = path.eval_single (gridpoints[i], 1); + assert(ndofs == v.size()); + + v_inv.noalias() = v.cwiseInverse(); + + value_type sdmin = - m_maxsd, + sdmax = m_maxsd; + for (Eigen::Index k = 0; k < ndofs; ++k) { + if (v[k] > 0) { + sdmax = std::min(m_upper[k] * v_inv[k], sdmax); + sdmin = std::max(m_lower[k] * v_inv[k], sdmin); + } else if (v[k] < 0) { + sdmax = std::min(m_lower[k] * v_inv[k], sdmax); + sdmin = std::max(m_upper[k] * v_inv[k], sdmin); + } else { + if (m_upper[k] < 0 || m_lower[k] > 0) + /// \todo the problem is infeasible. How should we inform the user ? + throw std::runtime_error("BoxConstraint is infeasible"); + } + } + xbound[i][0] = 0; + if (sdmin > 0) xbound[i][0] = sdmin*sdmin; + xbound[i][1] = sdmax * sdmax; + } +} + +} // namespace constraint +} // namespace toppra diff --git a/cpp/src/toppra/constraint/linear_joint_velocity.hpp b/cpp/src/toppra/constraint/linear_joint_velocity.hpp new file mode 100644 index 00000000..1f4d2473 --- /dev/null +++ b/cpp/src/toppra/constraint/linear_joint_velocity.hpp @@ -0,0 +1,47 @@ +#ifndef TOPPRA_CONSTRAINT_LINEAR_JOINT_VELOCITY_HPP +#define TOPPRA_CONSTRAINT_LINEAR_JOINT_VELOCITY_HPP + +#include + +namespace toppra { +namespace constraint { + +/// A Joint Velocity Constraint class. +class LinearJointVelocity : public LinearConstraint { + public: + LinearJointVelocity (const Vector& lowerVlimit, const Vector& upperVlimit) + : LinearConstraint (0, 0, true, false, true) + , m_lower (lowerVlimit) + , m_upper (upperVlimit) + , m_maxsd (1e8) + { + check(); + } + + /** Set the maximum allowed value of \f$\dot s\f$. + * \param maxsd should be strictly positive. + * */ + void maxSDot (value_type maxsd) + { + assert(maxsd > 0); + m_maxsd = maxsd; + } + + virtual std::ostream& print(std::ostream& os) const; + + private: + void check(); + + void computeParams_impl(const GeometricPath& path, + const Vector& gridpoints, + Vectors& a, Vectors& b, Vectors& c, + Matrices& F, Vectors& g, + Bounds& ubound, Bounds& xbound); + + Vector m_lower, m_upper; + value_type m_maxsd; +}; // class LinearJointVelocity +} // namespace constraint +} // namespace toppra + +#endif diff --git a/cpp/src/toppra/geometric_path.cpp b/cpp/src/toppra/geometric_path.cpp new file mode 100644 index 00000000..c5249a11 --- /dev/null +++ b/cpp/src/toppra/geometric_path.cpp @@ -0,0 +1,13 @@ + +#include "toppra/geometric_path.hpp" +namespace toppra { + +Vectors GeometricPath::eval(const Vector &positions, int order) const { + Vectors outputs; + outputs.resize(positions.size()); + for (size_t i = 0; i < positions.size(); i++) { + outputs[i] = eval_single(positions(i), order); + } + return outputs; + }; +} diff --git a/cpp/src/toppra/geometric_path.hpp b/cpp/src/toppra/geometric_path.hpp new file mode 100644 index 00000000..d48f1262 --- /dev/null +++ b/cpp/src/toppra/geometric_path.hpp @@ -0,0 +1,120 @@ +#ifndef TOPPRA_GEOMETRIC_PATH_HPP +#define TOPPRA_GEOMETRIC_PATH_HPP + +#include +#include +#include +#include +#include + +namespace toppra { + + +/** + * \brief Abstract interface for geometric paths. + */ +class GeometricPath { +public: + + GeometricPath() = default; + + /** + * Constructor of GeometricPath on vector spaces. + */ + GeometricPath(int nDof) : m_configSize(nDof), m_dof (nDof) {} + + /** + * Constructor of GeometricPath on non-vector spaces. + */ + GeometricPath(int configSize, int nDof) : m_configSize(configSize), m_dof (nDof) {} + + /** + * /brief Evaluate the path at given position. + */ + virtual Vector eval_single(value_type, int order = 0) const = 0; + + /** + * /brief Evaluate the path at given positions (vector). + * + * Default implementation: Evaluation each point one-by-one. + */ + virtual Vectors eval(const Vector &positions, int order = 0) const; + + /** + * \return the dimension of the configuration space + */ + int configSize() const + { + return m_configSize; + } + + /** + * \return the number of degrees-of-freedom of the path. + */ + int dof() const + { + return m_dof; + } + + /** + * \return the starting and ending path positions. + */ + virtual Bound pathInterval() const = 0; + + virtual ~GeometricPath () {} + +protected: + int m_configSize, m_dof; +}; + +/** + * \brief Piecewise polynomial geometric path. + * + * A simple implemetation of a piecewise polynoamial geometric path. + * + */ +class PiecewisePolyPath : public GeometricPath { +public: + + PiecewisePolyPath() = default; + + /** + * Consructor. + * + * @param coefficients Polynoamial coefficients. + * @param breakpoints Vector of breakpoints. + */ + PiecewisePolyPath(const Matrices& coefficients, std::vector breakpoints); + + /** + * /brief Evaluate the path at given position. + */ + Vector eval_single(value_type, int order = 0) const; + + /** + * /brief Evaluate the path at given positions (vector). + */ + Vectors eval(const Vector &, int order = 0) const; + + /** + * Return the starting and ending path positions. + */ + Bound pathInterval() const { + Bound v; + v << m_breakpoints.front(), m_breakpoints.back(); + return v; + }; + +private: + size_t findSegmentIndex(value_type pos) const; + void checkInputArgs(); + void computeDerivativesCoefficients(); + Matrix getCoefficient(int seg_index, int order) const; + Matrices m_coefficients, m_coefficients_1, m_coefficients_2; + std::vector m_breakpoints; + int m_degree; +}; + +} // namespace toppra + +#endif diff --git a/cpp/src/toppra/geometric_path/piecewise_poly_path.cpp b/cpp/src/toppra/geometric_path/piecewise_poly_path.cpp new file mode 100644 index 00000000..c0186293 --- /dev/null +++ b/cpp/src/toppra/geometric_path/piecewise_poly_path.cpp @@ -0,0 +1,103 @@ +#include "toppra/toppra.hpp" +#include +#include +#include +#include + +namespace toppra { + +Matrix differentiateCoefficients(const Matrix &coefficients) { + Matrix deriv(coefficients.rows(), coefficients.cols()); + deriv.setZero(); + for (size_t i = 1; i < coefficients.rows(); i++) { + deriv.row(i) = coefficients.row(i - 1) * (coefficients.rows() - i); + } + return deriv; +} + +PiecewisePolyPath::PiecewisePolyPath(const Matrices & coefficients, + std::vector breakpoints) + : GeometricPath (coefficients[0].cols()), + m_coefficients(coefficients), m_breakpoints(std::move(breakpoints)), + m_degree(coefficients[0].rows() - 1) { + + checkInputArgs(); + computeDerivativesCoefficients(); +} + +Vector PiecewisePolyPath::eval_single(value_type pos, int order) const { + Vector v(m_dof); + v.setZero(); + size_t seg_index = findSegmentIndex(pos); + Matrix coeff = getCoefficient(seg_index, order); + for (int power = 0; power < m_degree + 1; power++) { + v += coeff.row(power) * + pow(pos - m_breakpoints[seg_index], m_degree - power); + } + return v; +} + +// Not the most efficient implementation. Coefficients are +// recompoted. Should be refactorred. +Vectors PiecewisePolyPath::eval(const Vector &positions, int order) const { + Vectors outputs; + outputs.resize(positions.size()); + for (size_t i = 0; i < positions.size(); i++) { + outputs[i] = eval_single(positions(i), order); + } + return outputs; +} + +size_t PiecewisePolyPath::findSegmentIndex(value_type pos) const { + size_t seg_index = -1; + for (size_t i = 0; i < m_coefficients.size(); i++) { + if (m_breakpoints[i] <= pos && pos <= m_breakpoints[i + 1]) { + seg_index = i; + break; + } + } + if (seg_index == -1) { + std::ostringstream oss; + oss << "Position " << pos << " is outside of range [ " << m_breakpoints[0] + << ", " << m_breakpoints[m_breakpoints.size()-1] << ']'; + throw std::runtime_error(oss.str()); + } + return seg_index; +} + +void PiecewisePolyPath::checkInputArgs() { + if ((1 + m_coefficients.size()) != m_breakpoints.size()) { + throw std::runtime_error( + "Number of breakpoints must equals number of segments plus 1."); + } + for (size_t seg_index = 0; seg_index < m_coefficients.size(); seg_index++) { + if (m_breakpoints[seg_index] >= m_breakpoints[seg_index + 1]) { + throw std::runtime_error("Require strictly increasing breakpoints"); + } + } +} + +void PiecewisePolyPath::computeDerivativesCoefficients() { + m_coefficients_1.reserve(m_coefficients.size()); + m_coefficients_2.reserve(m_coefficients.size()); + for (size_t seg_index = 0; seg_index < m_coefficients.size(); seg_index++) { + m_coefficients_1.push_back( + differentiateCoefficients(m_coefficients[seg_index])); + m_coefficients_2.push_back( + differentiateCoefficients(m_coefficients_1[seg_index])); + } +} + +Matrix PiecewisePolyPath::getCoefficient(int seg_index, int order) const { + Matrix coeff; + if (order == 0) { + coeff = m_coefficients[seg_index]; + } else if (order == 1) { + coeff = m_coefficients_1[seg_index]; + } else if (order == 2) { + coeff = m_coefficients_2[seg_index]; + } + return coeff; +} + +} // namespace toppra diff --git a/cpp/src/toppra/solver.cpp b/cpp/src/toppra/solver.cpp new file mode 100644 index 00000000..3abace94 --- /dev/null +++ b/cpp/src/toppra/solver.cpp @@ -0,0 +1,46 @@ +#include + +#include +#include + +namespace toppra { + +Solver::Solver (const LinearConstraintPtrs& constraints, const GeometricPath& path, + const Vector& times) + : m_constraints (constraints) + , m_path (path) + , m_times (times) + , m_N (times.size()-1) + , m_nV (2) + , m_deltas (times.tail(m_N) - times.head(m_N)) +{ + if ((m_deltas.array() <= 0).any()) + throw std::invalid_argument("Invalid times."); + /// \todo assert that the time range of the path equals [ times[0], times[m_N] ]. + + // Compute the constraints parameters + LinearConstraintParams emptyLinParams; + BoxConstraintParams emptyBoxParams; + for (std::size_t c = 0; c < m_constraints.size(); ++c) { + LinearConstraint* lin = m_constraints[c].get(); + LinearConstraintParams* lparam; + if (lin->hasLinearInequalities()) { + m_constraintsParams.lin.emplace_back(); + lparam = &m_constraintsParams.lin.back(); + lparam->cid = c; + } else + lparam = &emptyLinParams; + BoxConstraintParams* bparam; + if (lin->hasUbounds() || lin->hasXbounds()) { + m_constraintsParams.box.emplace_back(); + bparam = &m_constraintsParams.box.back(); + bparam->cid = c; + } else + bparam = &emptyBoxParams; + lin->computeParams(path, times, + lparam->a, lparam->b, lparam->c, lparam->F, lparam->g, + bparam->u, bparam->x); + } +} + +} // namespace toppra diff --git a/cpp/src/toppra/solver.hpp b/cpp/src/toppra/solver.hpp new file mode 100644 index 00000000..a3e08fb9 --- /dev/null +++ b/cpp/src/toppra/solver.hpp @@ -0,0 +1,124 @@ +#ifndef TOPPRA_SOLVER_HPP +#define TOPPRA_SOLVER_HPP + +#include + +namespace toppra { + +/** \brief The base class for all solver wrappers. + * + * All Solver can solve Linear/Quadratic Program subject to linear constraints + * at the given stage, and possibly with additional auxiliary constraints. + * + * All Solver derived class implements + * - Solver::solveStagewiseOptim: core method needed by all Reachability + * Analysis-based algorithms + * - Solver::setupSolver, Solver::closeSolver: needed by some Solver + * implementation, such as mosek and qpOASES with warmstart. + * + * Note that some Solver only handle Linear Program while + * some handle both. + * + * Each solver wrapper should provide solver-specific constraint, + * such as ultimate bound the variable u, x. For some solvers such as + * ECOS, this is very important. + * + * */ +class Solver { + public: + /// \copydoc Solver::m_deltas + const Vector& deltas () const + { + return m_deltas; + } + + /// \copydoc Solver::m_N + std::size_t nbStages () const + { + return m_N; + } + + /// \copydoc Solver::m_nV + std::size_t nbVars () const + { + return m_nV; + } + + /** Solve a stage-wise quadratic (or linear) optimization problem. + * + * The quadratic optimization problem is described below: + * + * \f{eqnarray} + * \text{min } & 0.5 [u, x, v] H [u, x, v]^\top + [u, x, v] g \\ + * \text{s.t. } & [u, x] \text{ is feasible at stage } i \\ + * & x_{min} \leq x \leq x_{max} \\ + * & x_{next, min} \leq x + 2 \Delta_i u \leq x_{next, max}, + * \f} + * + * where `v` is an auxiliary variable, only exist if there are + * non-canonical constraints. The linear program is the + * quadratic problem without the quadratic term. + * + * \param i The stage index. + * \param H Either a matrix of size (d, d), where d is \ref nbVars, in + * which case a quadratic objective is defined, or a matrix + * of size (0,0), in which case a linear objective is defined. + * \param g Vector of size \ref nbVars. The linear term. + * \param[out] solution in case of success, stores the optimal solution. + * + * \return whether the resolution is successful, in which case \c solution + * contains the optimal solution. + * */ + virtual bool solveStagewiseOptim(std::size_t i, + const Matrix& H, const Vector& g, + const Bound& x, const Bound& xNext, + Vector& solution) = 0; + + /** \brief Initialize the wrapped solver + */ + virtual void setupSolver () + {} + + /** \brief Free the wrapped solver + */ + virtual void closeSolver () + {} + + virtual ~Solver () {} + + protected: + Solver (const LinearConstraintPtrs& constraints, const GeometricPath& path, + const Vector& times); + + struct LinearConstraintParams { + int cid; + Vectors a, b, c, g; + Matrices F; + }; + struct BoxConstraintParams { + int cid; + Bounds u, x; + }; + struct ConstraintsParams { + std::vector lin; + std::vector box; + } m_constraintsParams; + + LinearConstraintPtrs m_constraints; + const GeometricPath& m_path; + Vector m_times; + + private: + /// \brief Number of stages. + /// The number of gridpoints equals N + 1, where N is the number of stages. + std::size_t m_N; + /// Total number of variables, including u, x. + std::size_t m_nV; + /// Time increment between each stage. Size \ref nbStages + Vector m_deltas; + +}; // class Solver + +} // namespace toppra + +#endif diff --git a/cpp/src/toppra/solver/glpk-wrapper.cpp b/cpp/src/toppra/solver/glpk-wrapper.cpp new file mode 100644 index 00000000..2d0500e8 --- /dev/null +++ b/cpp/src/toppra/solver/glpk-wrapper.cpp @@ -0,0 +1,136 @@ +#include + +#include + +#include + +namespace toppra { +namespace solver { + +void intersection (const Bound& a, const Bound& b, Bound& c) +{ + c[0] = std::max(a[0], b[0]); + c[1] = std::min(a[1], b[1]); +} + +void set_col_bnds(glp_prob* lp, int i, const Bound& ub) +{ + glp_set_col_bnds(lp, i, (ub[0] == -infty + ? (ub[1] == infty ? GLP_FR : GLP_UP) + : (ub[1] == infty ? GLP_LO : GLP_DB) + ), ub[0], ub[1]); +} + +void set_row_bnds(glp_prob* lp, int i, const value_type& l, const value_type& u) +{ + glp_set_row_bnds(lp, i, (l == -infty + ? (u == infty ? GLP_FR : GLP_UP) + : (u == infty ? GLP_LO : GLP_DB) + ), l, u); +} +void set_row_bnds(glp_prob* lp, int i, const Bound& b) { set_row_bnds(lp, i, b[0], b[1]); } + +GLPKWrapper::GLPKWrapper (const LinearConstraintPtrs& constraints, const GeometricPath& path, + const Vector& times) + : Solver (constraints, path, times) + , m_lp (glp_create_prob()) +{ + // Currently only support Canonical Linear Constraint + assert(nbVars() == 2); + + glp_set_obj_dir(m_lp, GLP_MIN); + + // auxiliary variables + glp_add_cols(m_lp, 2); + glp_set_col_name(m_lp, 1, "u"); + glp_set_col_name(m_lp, 2, "x"); + glp_set_col_bnds(m_lp, 1, GLP_FR, 0., 0.); + + // structural variables + int nC = 1; + for (const Solver::LinearConstraintParams& linParam : m_constraintsParams.lin) + nC += linParam.F[0].rows(); + + glp_add_rows(m_lp, nC); + glp_set_row_name(m_lp, 1, "x_next"); +} + +GLPKWrapper::~GLPKWrapper () +{ + glp_delete_prob(m_lp); +} + +bool GLPKWrapper::solveStagewiseOptim(std::size_t i, + const Matrix& H, const Vector& g, + const Bound& x, const Bound& xNext, + Vector& solution) +{ + if (H.size() > 0) + throw std::invalid_argument("GLPK can only solve LPs."); + + int N (nbStages()); + assert (i <= N); + + const int ind[] = {0, 1, 2}; + double val[] = {0., 0., 1.}; + + if (i < N) { + // x_next row + val[1] = 2*deltas()[i]; + glp_set_mat_row(m_lp, 1, 2, ind, val); + set_row_bnds(m_lp, 1, xNext); + } else { + glp_set_mat_row(m_lp, 1, 0, NULL, NULL); + glp_set_row_bnds(m_lp, 1, GLP_FR, 0., 0.); + } + + int iC = 2; + for (const Solver::LinearConstraintParams& lin : m_constraintsParams.lin) + { + std::size_t j (lin.F.size() == 1 ? 0 : i); + const Matrix& _F (lin.F[j]); + const Vector& _g (lin.g[j]); + int nC (_F.rows()); + + for (int k = 0; k < _F.rows(); ++k) { + val[1] = _F.row(k) * lin.a[i]; + val[2] = _F.row(k) * lin.b[i]; + glp_set_mat_row(m_lp, iC, 2, ind, val); + set_row_bnds(m_lp, iC, -infty, _g[k] - _F.row(k) * lin.c[i]); + ++iC; + } + } + + // Bounds on x and u + Bound xb(x), ub; + ub << -infty, infty; + + for (const Solver::BoxConstraintParams& box : m_constraintsParams.box) + { + if (!box.u.empty()) intersection(ub, box.u[i], ub); + if (!box.x.empty()) intersection(xb, box.x[i], xb); + } + + set_col_bnds(m_lp, 1, ub); + set_col_bnds(m_lp, 2, xb); + + // Fill cost + glp_set_obj_coef(m_lp, 1, g[0]); + glp_set_obj_coef(m_lp, 2, g[1]); + + // TODO give a try to glp_interior + glp_smcp parm; + glp_init_smcp(&parm); + parm.msg_lev = GLP_MSG_ERR; + int ret = glp_simplex(m_lp, &parm); + if (ret == 0) { + solution.resize(2); + solution << glp_get_col_prim(m_lp, 1), glp_get_col_prim(m_lp, 2); + return true; + } + std::cout << ret << std::endl; + return false; +} + +} // namespace solver +} // namespace toppra diff --git a/cpp/src/toppra/solver/glpk-wrapper.hpp b/cpp/src/toppra/solver/glpk-wrapper.hpp new file mode 100644 index 00000000..ecdfa33f --- /dev/null +++ b/cpp/src/toppra/solver/glpk-wrapper.hpp @@ -0,0 +1,52 @@ +#ifndef TOPPRA_SOLVER_GLPK_WRAPPER_HPP +#define TOPPRA_SOLVER_GLPK_WRAPPER_HPP + +#include +#include + +// Forward declare glpk solver. +struct glp_prob; + +namespace toppra { +namespace solver { + +/** Wrapper around GLPK library. + * + * Internally, the problem is formulated as + * \f{eqnarray} + * min & g^T y \\ + * s.t & z = A y \\ + * & x_{min} <= x <= x_{max} \\ + * & l_1 <= z <= h_1 \\ + * \f} + * where + * \f{eqnarray} + * y =& \begin{pmatrix} u & x \end{pmatrix}^T \\ + * A =& \begin{pmatrix} + * 2\delta & 1 \\ + * F_i a_i & F_i b_i \\ + * \vdots & \vdots \\ + * \end{pmatrix}\\ + * \f} + * + * */ +class GLPKWrapper : public Solver { + public: + GLPKWrapper (const LinearConstraintPtrs& constraints, const GeometricPath& path, + const Vector& times); + + bool solveStagewiseOptim(std::size_t i, + const Matrix& H, const Vector& g, + const Bound& x, const Bound& xNext, + Vector& solution); + + virtual ~GLPKWrapper(); + + private: + glp_prob* m_lp; +}; // class GLPKWrapper + +} // namespace solver +} // namespace toppra + +#endif diff --git a/cpp/src/toppra/solver/qpOASES-wrapper.cpp b/cpp/src/toppra/solver/qpOASES-wrapper.cpp new file mode 100644 index 00000000..ca9de533 --- /dev/null +++ b/cpp/src/toppra/solver/qpOASES-wrapper.cpp @@ -0,0 +1,143 @@ +#include +#include + +#include + +namespace toppra { +namespace solver { +struct qpOASESWrapper::Impl { + qpOASES::SQProblem qp; + + Impl(Eigen::Index nV, Eigen::Index nC) + : qp (nV, nC) + { + qpOASES::Options options; + options.printLevel = qpOASES::PL_NONE; + + qp.setOptions( options ); + } +}; + +value_type qpOASESWrapper::m_defaultBoundary = 1e16; + +void qpOASESWrapper::setDefaultBoundary (const value_type& v) +{ + m_defaultBoundary = v; +} + +qpOASESWrapper::qpOASESWrapper (const LinearConstraintPtrs& constraints, const GeometricPath& path, + const Vector& times) + : Solver (constraints, path, times) + , m_boundary (m_defaultBoundary) +{ + // Currently only support Canonical Linear Constraint + Eigen::Index nC = 2; // First constraint is x + 2 D u <= xnext_max, second is xnext_min <= x + 2D u + for (const Solver::LinearConstraintParams& linParam : m_constraintsParams.lin) + nC += linParam.F[0].rows(); + + Eigen::Index nV (nbVars()); + assert(nV == 2); + m_A = RMatrix::Zero(nC, nV); + m_lA = -Vector::Ones(nC); + m_hA = -Vector::Ones(nC); + + m_impl = std::unique_ptr(new Impl(nV, nC)); +} + +qpOASESWrapper::~qpOASESWrapper () +{ +} + +bool qpOASESWrapper::solveStagewiseOptim(std::size_t i, + const Matrix& H, const Vector& g, + const Bound& x, const Bound& xNext, + Vector& solution) +{ + TOPPRA_LOG_DEBUG("stage: i="<().setZero(); + m_lA.head<2>().setConstant(-1); + m_hA.head<2>().setOnes(); + } + Eigen::Index cur_index = 2; + for (const Solver::LinearConstraintParams& lin : m_constraintsParams.lin) + { + std::size_t j (lin.F.size() == 1 ? 0 : i); + const Matrix& _F (lin.F[j]); + const Vector& _g (lin.g[j]); + Eigen::Index nC (_F.rows()); + + m_A.block(cur_index, 0, nC, 1) = _F * lin.a[i]; + m_A.block(cur_index, 1, nC, 1) = _F * lin.b[i]; + m_hA.segment(cur_index, nC) = _g - _F * lin.c[i]; + m_lA.segment(cur_index, nC).setConstant(-m_boundary); + cur_index += nC; + } + for (const Solver::BoxConstraintParams& box : m_constraintsParams.box) + { + if (!box.u.empty()) { + l[0] = std::max(l[0], box.u[i][0]); + h[0] = std::min(h[0], box.u[i][1]); + } + if (!box.x.empty()) { + l[1] = std::max(l[1], box.x[i][0]); + h[1] = std::min(h[1], box.x[i][1]); + } + } + + TOPPRA_LOG_DEBUG("lA: " << std::endl << m_lA); + TOPPRA_LOG_DEBUG("hA: " << std::endl << m_hA); + TOPPRA_LOG_DEBUG(" A: " << std::endl << m_A); + TOPPRA_LOG_DEBUG("l : " << std::endl << l); + TOPPRA_LOG_DEBUG("h : " << std::endl << h); + + qpOASES::returnValue res; + // TODO I assumed 1000 is the argument nWSR of the SQProblem.init function. + //res = self.solver.init( + // H, g, self._A, l, h, self._lA, self._hA, np.array([1000]) + //) + int nWSR = 1000; + if (H.size() == 0) { + m_impl->qp.setHessianType(qpOASES::HST_ZERO); + res = m_impl->qp.init (NULL, g.data(), + m_A.data(), + l.data(), h.data(), + m_lA.data(), m_hA.data(), + nWSR); + } else { + m_H = H; // Convert to row-major + res = m_impl->qp.init (m_H.data(), g.data(), + m_A.data(), + l.data(), h.data(), + m_lA.data(), m_hA.data(), + nWSR); + } + + if (res == qpOASES::SUCCESSFUL_RETURN) { + solution.resize(nbVars()); + m_impl->qp.getPrimalSolution(solution.data()); + return true; + } + return false; +} + +} // namespace solver +} // namespace toppra diff --git a/cpp/src/toppra/solver/qpOASES-wrapper.hpp b/cpp/src/toppra/solver/qpOASES-wrapper.hpp new file mode 100644 index 00000000..9fa550b7 --- /dev/null +++ b/cpp/src/toppra/solver/qpOASES-wrapper.hpp @@ -0,0 +1,64 @@ +#ifndef TOPPRA_SOLVER_QPOASES_WRAPPER_HPP +#define TOPPRA_SOLVER_QPOASES_WRAPPER_HPP + +#include +#include + +namespace toppra { +namespace solver { + +/** Wrapper around qpOASES::SQProblem + * + * Internally, the problem is formulated as + * \f{eqnarray} + * min & 0.5 y^T H y + g^T y \\ + * s.t & lA <= Ay <= hA \\ + * & l <= y <= h \\ + * \f} + * + * \todo Add a solver that inherits from qpOASESWrapper and that uses the warm + * start capabilities of qpOASES + * + * */ +class qpOASESWrapper : public Solver { + public: + qpOASESWrapper (const LinearConstraintPtrs& constraints, const GeometricPath& path, + const Vector& times); + + bool solveStagewiseOptim(std::size_t i, + const Matrix& H, const Vector& g, + const Bound& x, const Bound& xNext, + Vector& solution); + + virtual ~qpOASESWrapper(); + + value_type setBoundary () const + { + return m_boundary; + } + + void setBoundary (const value_type& v) + { + m_boundary = v; + } + + static void setDefaultBoundary (const value_type& v); + + private: + /// qpOASES uses row-major storage order. + typedef Eigen::Matrix RMatrix; + RMatrix m_H, m_A; + Vector m_lA, m_hA; + value_type m_boundary; + + static value_type m_defaultBoundary; + + struct Impl; + std::unique_ptr m_impl; +}; // class qpOASESWrapper + +} // namespace solver +} // namespace toppra + +#endif diff --git a/cpp/src/toppra/toppra.hpp b/cpp/src/toppra/toppra.hpp new file mode 100644 index 00000000..37ee54e7 --- /dev/null +++ b/cpp/src/toppra/toppra.hpp @@ -0,0 +1,63 @@ +#ifndef TOPPRA_TOPPRA_HPP +#define TOPPRA_TOPPRA_HPP + +#include +#include +#include +#include + +#include +#include + +#include + +#ifdef TOPPRA_DEBUG_ON +#define TOPPRA_LOG_DEBUG(X) std::cout << "[DEBUG]: " << X << std::endl +#else +#define TOPPRA_LOG_DEBUG(X) ((void)0) +#endif + + +/// The TOPP-RA namespace +namespace toppra { + /// The scalar type + typedef double value_type; + + constexpr value_type infty = std::numeric_limits::infinity(); + + /// Column vector type + typedef Eigen::Matrix Vector; + /// Matrix type + typedef Eigen::Matrix Matrix; + + /// Vector of Vector + typedef std::vector > Vectors; + /// Vector of Matrix + typedef std::vector > Matrices; + + /// 2D vector that stores the upper and lower bound of a variable. + typedef Eigen::Matrix Bound; + /// Vector of Bound + typedef std::vector > Bounds; + + class LinearConstraint; + /// Shared pointer to a LinearConstraint + typedef std::shared_ptr LinearConstraintPtr; + /// Vector of LinearConstraintPtr + typedef std::vector LinearConstraintPtrs; + namespace constraint { + class LinearJointVelocity; + class LinearJointAcceleration; + class JointTorque; + } // namespace constraint + + class Solver; + typedef std::shared_ptr SolverPtr; + namespace solver { + class qpOASESWrapper; + } // namespace solver + + class GeometricPath; +} // namespace toppra + +#endif diff --git a/cpp/tests/CMakeLists.txt b/cpp/tests/CMakeLists.txt new file mode 100644 index 00000000..e1e58307 --- /dev/null +++ b/cpp/tests/CMakeLists.txt @@ -0,0 +1,25 @@ + +# Now simply link against gtest or gtest_main as needed. Eg +set(TEST_SOURCES + gtest_main.cpp + test_constraints.cpp + test_solver.cpp + test_poly_geometric_path.cpp + test_algorithm.cpp + ) + +add_executable(all_tests ${TEST_SOURCES}) +target_link_libraries(all_tests PUBLIC toppra gtest ${CMAKE_THREAD_LIBS_INIT}) + +if(BUILD_WITH_PINOCCHIO) + target_compile_definitions(all_tests PRIVATE -DBUILD_WITH_PINOCCHIO) + target_link_libraries(all_tests PUBLIC pinocchio::pinocchio) +endif() +if(BUILD_WITH_qpOASES) + target_compile_definitions(all_tests PRIVATE -DBUILD_WITH_qpOASES) +endif() +if(BUILD_WITH_GLPK) + target_compile_definitions(all_tests PRIVATE -DBUILD_WITH_GLPK) +endif() + +add_test(NAME all_tests COMMAND all_tests) diff --git a/cpp/tests/gtest_main.cpp b/cpp/tests/gtest_main.cpp new file mode 100644 index 00000000..4483c91a --- /dev/null +++ b/cpp/tests/gtest_main.cpp @@ -0,0 +1,6 @@ +#include "gtest/gtest.h" + +int main(int argc, char **argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/cpp/tests/test_algorithm.cpp b/cpp/tests/test_algorithm.cpp new file mode 100644 index 00000000..16bc6239 --- /dev/null +++ b/cpp/tests/test_algorithm.cpp @@ -0,0 +1,153 @@ +#include +#include +#include +#include +#include +#include +#include +#include "toppra/algorithm.hpp" + +#include "gtest/gtest.h" + +#define TEST_PRECISION 1e-6 + +// clang-format off + +/* Code use to generate the test scenario using the Python implementation +// Diable format to keep python code identation + +import toppra as ta +import numpy as np + +path = ta.SplineInterpolator([0, 1, 2, 3], [[0, 0], [1, 3], [2, 4], [0, 0]]) + +def print_cpp_code(p): + out = "" + for seg_idx in range(p.cspl.c.shape[1]): + out += "coeff{:d} << ".format(seg_idx) + for i, t in enumerate(p.cspl.c[:, seg_idx, :].flatten().tolist()): + if i == len(p.cspl.c[:, seg_idx, :].flatten().tolist()) - 1: + out += "{:f};\n".format(t) + else: + out += "{:f}, ".format(t) + return out + +print(print_cpp_code(path)) +print("breakpoints: {}".format([0, 1, 2, 3])) +x_eval = [0, 0.5, 1., 1.1, 2.5] +print("Eval for x_eval = {:}\npath(x_eval)=\n{}\npath(x_eval, 1)=\n{}\npath(x_eval, 2)=\n{}".format( + x_eval, path(x_eval), path(x_eval, 1), path(x_eval, 2))) + +pc_vel = ta.constraint.JointVelocityConstraint([1.0, 1.0]) +pc_acc = ta.constraint.JointAccelerationConstraint([0.2, 0.2], discretization_scheme=0) + +instance = ta.algorithm.TOPPRA([pc_vel, pc_acc], path, gridpoints=np.linspace(0, 3, 51), solver_wrapper='qpoases') +sdds, sds, _, K = instance.compute_parameterization(0, 0, return_data=True) +feasible_sets = instance.compute_feasible_sets(). + + */ +// clang-format on + +class ProblemInstance : public testing::Test { + public: + ProblemInstance() { + toppra::Matrix coeff0{4, 2}, coeff1{4, 2}, coeff2{4, 2}; + coeff0 << -0.500000, -0.500000, 1.500000, 0.500000, 0.000000, 3.000000, 0.000000, + 0.000000; + coeff1 << -0.500000, -0.500000, 0.000000, -1.000000, 1.500000, 2.500000, 1.000000, + 3.000000; + coeff2 << -0.500000, -0.500000, -1.500000, -2.500000, 0.000000, -1.000000, 2.000000, + 4.000000; + toppra::Matrices coefficents = {coeff0, coeff1, coeff2}; + path = toppra::PiecewisePolyPath(coefficents, std::vector{0, 1, 2, 3}); + v = toppra::LinearConstraintPtrs{ + std::make_shared( + -toppra::Vector::Ones(nDof), toppra::Vector::Ones(nDof)), + std::make_shared( + -0.2 * toppra::Vector::Ones(nDof), 0.2 * toppra::Vector::Ones(nDof))}; + }; + + toppra::PiecewisePolyPath path; + toppra::LinearConstraintPtrs v; + int nDof = 2; +}; + +TEST_F(ProblemInstance, GridpointsHasCorrectShape) { + toppra::algorithm::TOPPRA problem{v, path}; + problem.setN(50); + problem.computePathParametrization(); + auto data = problem.getParameterizationData(); + + ASSERT_EQ(data.gridpoints.size(), 51); + ASSERT_TRUE(data.gridpoints.isApprox(toppra::Vector::LinSpaced(51, 0, 3))); +} + +TEST_F(ProblemInstance, ControllableSets) { + toppra::algorithm::TOPPRA problem{v, path}; + problem.setN(50); + auto ret_code = problem.computePathParametrization(); + auto data = problem.getParameterizationData(); + toppra::Vector e_K_max(51); + e_K_max << 0.06666667, 0.07624309, 0.08631706, 0.09690258, 0.1005511, 0.09982804, + 0.09979021, 0.1004364, 0.10178673, 0.10184412, 0.09655088, 0.09173679, 0.08734254, + 0.08331796, 0.07962037, 0.07621325, 0.07306521, 0.07014913, 0.0674415, 0.06492188, + 0.06257244, 0.06037764, 0.05832397, 0.05639984, 0.05459563, 0.05290407, + 0.05132158, 0.04985238, 0.04852317, 0.04745694, 0.04761905, 0.05457026, + 0.06044905, 0.06527948, 0.08479263, 0.10990991, 0.13252362, 0.15269631, + 0.15777077, 0.12111776, 0.09525987, 0.07641998, 0.06232537, 0.05154506, + 0.04314353, 0.03257513, 0.02268898, 0.01495548, 0.0088349, 0.00394283, 0.; + + ASSERT_EQ(ret_code, toppra::ReturnCode::OK) + << "actual return code: " << (int)ret_code; + for (int i = 0; i < 51; i++) + EXPECT_NEAR(data.controllable_sets(i, 1), e_K_max(i), TEST_PRECISION) + << "idx: " << i; +} + +TEST_F(ProblemInstance, OutputParmetrization) { + toppra::algorithm::TOPPRA problem{v, path}; + problem.setN(50); + auto ret_code = problem.computePathParametrization(); + auto data = problem.getParameterizationData(); + toppra::Vector expected_parametrization(51); + expected_parametrization << 0., 0.00799999, 0.01559927, 0.02295854, 0.03021812, + 0.0375065, 0.04494723, 0.05266502, 0.06079176, 0.06947278, 0.07887417, 0.08890758, + 0.08734253, 0.08331795, 0.07962036, 0.07621324, 0.0730652, 0.07014912, 0.06744149, + 0.06492187, 0.06257243, 0.06037763, 0.05832396, 0.05639983, 0.05459562, + 0.05290406, 0.05132157, 0.04985237, 0.04852316, 0.04745693, 0.04761904, 0.0285715, + 0.05376003, 0.04275653, 0.04126188, 0.04013804, 0.03912958, 0.03818766, + 0.03729606, 0.0364472, 0.03563649, 0.03486069, 0.03411724, 0.03340395, 0.03271895, + 0.03206054, 0.02268897, 0.01495547, 0.00883489, 0.00394282, 0.; + + ASSERT_EQ(ret_code, toppra::ReturnCode::OK) + << "actual return code: " << (int)ret_code; + + for (int i = 0; i < 51; i++) + EXPECT_NEAR(data.parametrization(i), expected_parametrization(i), TEST_PRECISION) + << "idx: " << i + << ", abs diff=" << data.parametrization(i) - expected_parametrization(i); +} + +TEST_F(ProblemInstance, FeasibleSets) { + toppra::algorithm::TOPPRA problem{v, path}; + problem.setN(50); + auto ret_code = problem.computeFeasibleSets(); + auto data = problem.getParameterizationData(); + toppra::Vector expected_feasible_max(51); + expected_feasible_max << 0.06666667, 0.07624309, 0.08631706, 0.09690258, 0.1005511, + 0.09982804, 0.09979021, 0.1004364, 0.10178673, 0.10388394, 0.10679654, 0.11062383, + 0.11550389, 0.12162517, 0.12924407, 0.13871115, 0.15051124, 0.16532619, + 0.18413615, 0.20838854, 0.24029219, 0.27052997, 0.2601227, 0.2447933, 0.22462845, + 0.2, 0.17154989, 0.14013605, 0.10674847, 0.07241209, 0.04761905, 0.05457026, + 0.06044905, 0.06527948, 0.08479263, 0.10990991, 0.13252362, 0.15269631, + 0.15777077, 0.12111776, 0.09525987, 0.07641998, 0.06232537, 0.05154506, + 0.04314353, 0.03648939, 0.0311448, 0.02679888, 0.02322632, 0.02026086, 0.01777778; + + ASSERT_EQ(ret_code, toppra::ReturnCode::OK) + << "actual return code: " << (int)ret_code; + + for (int i = 0; i < 51; i++) + EXPECT_NEAR(data.feasible_sets(i, 1), expected_feasible_max(i), TEST_PRECISION) + << "idx: " << i + << ", abs diff=" << data.parametrization(i) - expected_feasible_max(i); +} diff --git a/cpp/tests/test_constraints.cpp b/cpp/tests/test_constraints.cpp new file mode 100644 index 00000000..575d8eda --- /dev/null +++ b/cpp/tests/test_constraints.cpp @@ -0,0 +1,96 @@ +#include + +#ifdef BUILD_WITH_PINOCCHIO +#include +#include +#endif + +#include + +#include "gtest/gtest.h" + +class Constraint : public testing::Test { +public: + Constraint() : path(constructPath()) { + + // path has the equation: 0 * x ^ 3 + 1 * x ^ 2 + 2 x ^ 1 + 3 + } + + toppra::PiecewisePolyPath constructPath() { + toppra::Matrix coeff{4, 6}; + coeff.colwise() = toppra::Vector::LinSpaced(4, 0, 3); + toppra::Matrices coefficents = {coeff, coeff}; + toppra::PiecewisePolyPath p = + toppra::PiecewisePolyPath(coefficents, std::vector{0, 1, 2}); + return p; + } + + toppra::PiecewisePolyPath path; +}; + +TEST_F(Constraint, LinearJointVelocity) { + using namespace toppra; + int nDof = path.dof(); + Vector lb (-Vector::Ones(nDof)), + ub ( Vector::Ones(nDof)); + constraint::LinearJointVelocity ljv (lb, ub); + + EXPECT_TRUE(ljv.constantF()); + EXPECT_EQ(ljv.discretizationType(), Collocation); + EXPECT_EQ(ljv.nbConstraints(), 0); + EXPECT_EQ(ljv.nbVariables(), 0); + + int N = 10; + Vector gridpoints; + { + Bound I (path.pathInterval()); + gridpoints = toppra::Vector::LinSpaced (N, I[0], I[1]); + } + { + Vectors a, b, c, g; + Matrices F; + Bounds ub, xb; + ljv.allocateParams(gridpoints.size(), a, b, c, F, g, ub, xb); + EXPECT_EQ(a .size(), 0); + EXPECT_EQ(b .size(), 0); + EXPECT_EQ(c .size(), 0); + EXPECT_EQ(F .size(), 0); + EXPECT_EQ(g .size(), 0); + EXPECT_EQ(ub.size(), 0); + EXPECT_EQ(xb.size(), N); + ljv.computeParams(path, gridpoints, a, b, c, F, g, ub, xb); + } +} + +#ifdef BUILD_WITH_PINOCCHIO +TEST_F(Constraint, jointTorquePinocchio) { + using namespace toppra; + typedef constraint::jointTorque::Pinocchio<> JointTorque; + + JointTorque::Model model; + pinocchio::buildModels::manipulator(model); +#if PINOCCHIO_VERSION_AT_MOST(2,4,1) + // Work around bug solved by https://github.com/stack-of-tasks/pinocchio/pull/1155 + model.effortLimit.setConstant(10); +#endif + Vector frictions (Vector::Constant(model.nv, 0.001)); + JointTorque constraint (model, frictions); + + EXPECT_EQ(constraint.nbVariables(), model.nv); + EXPECT_EQ(constraint.nbConstraints(), 2*model.nv); + + int N = 10; + Vector gridpoints; + { + Bound I (path.pathInterval()); + gridpoints = toppra::Vector::LinSpaced (N, I[0], I[1]); + } + + { + Vectors a, b, c, g; + Matrices F; + Bounds ub, xb; + constraint.computeParams(path, gridpoints, a, b, c, F, g, ub, xb); + } +} +#endif diff --git a/cpp/tests/test_poly_geometric_path.cpp b/cpp/tests/test_poly_geometric_path.cpp new file mode 100644 index 00000000..00dccfaf --- /dev/null +++ b/cpp/tests/test_poly_geometric_path.cpp @@ -0,0 +1,255 @@ +#include "gtest/gtest.h" +#include +#include +#include +#include + +#include + +#include "toppra/geometric_path.hpp" +#include "toppra/toppra.hpp" + +class ConstructPiecewisePoly : public testing::Test { +public: + ConstructPiecewisePoly() : path(constructPath()) { + + // path has the equation: 0 * x ^ 3 + 1 * x ^ 2 + 2 x ^ 1 + 3 + } + + toppra::PiecewisePolyPath constructPath() { + toppra::Matrix coeff{4, 2}; + coeff(0, 0) = 0; + coeff(0, 1) = 0; + coeff(1, 0) = 1; + coeff(1, 1) = 1; + coeff(2, 0) = 2; + coeff(2, 1) = 2; + coeff(3, 0) = 3; + coeff(3, 1) = 3; + toppra::Matrices coefficents = {coeff, coeff}; + toppra::PiecewisePolyPath p = + toppra::PiecewisePolyPath(coefficents, std::vector{0, 1, 2}); + return p; + } + + toppra::PiecewisePolyPath path; +}; + +TEST_F(ConstructPiecewisePoly, OutputValueHasCorrectDOF) { + toppra::Vector pos = path.eval_single(0.5, 0); + ASSERT_EQ(pos.rows(), 2); +} + +TEST_F(ConstructPiecewisePoly, CorrectOutputValueFirstSegment) { + toppra::Vector pos = path.eval_single(0.5, 0); + ASSERT_DOUBLE_EQ(pos(0), 1 * pow(0.5, 2) + 2 * pow(0.5, 1) + 3); +} + +TEST_F(ConstructPiecewisePoly, CorrectOutputValueSecondSegment) { + toppra::Vector pos = path.eval_single(1.5, 0); + ASSERT_DOUBLE_EQ(pos(0), 1 * pow(0.5, 2) + 2 * pow(0.5, 1) + 3); +} + +TEST_F(ConstructPiecewisePoly, ThrowWhenOutOfrange) { + toppra::value_type out_of_range_pos{10}; + ASSERT_THROW(path.eval_single(out_of_range_pos, 0), std::runtime_error); +} + +TEST_F(ConstructPiecewisePoly, DeriveDerivativeOfCoefficients) { + toppra::value_type out_of_range_pos{10}; + ASSERT_THROW(path.eval_single(out_of_range_pos, 0), std::runtime_error); +} + +TEST_F(ConstructPiecewisePoly, CorrectDerivative) { + toppra::Vector pos = path.eval_single(0.5, 1); + ASSERT_DOUBLE_EQ(pos(0), 2 * pow(0.5, 1) + 2 * 1 + 0); +} + +TEST_F(ConstructPiecewisePoly, CorrectDoubldDerivative) { + toppra::Vector pos = path.eval_single(0.5, 2); + ASSERT_DOUBLE_EQ(pos(0), 2 * 1 + 0 + 0); +} + +TEST_F(ConstructPiecewisePoly, ComputeManyPointsEigen) { + toppra::Vector times{2}; + times << 0.5, 1.2; + toppra::Vectors positions = path.eval(times, 0); + ASSERT_DOUBLE_EQ(positions[0](0), 1 * pow(0.5, 2) + 2 * pow(0.5, 1) + 3); + ASSERT_DOUBLE_EQ(positions[1](0), 1 * pow(0.2, 2) + 2 * pow(0.2, 1) + 3); +} + +TEST_F(ConstructPiecewisePoly, CorrectDOF) { + ASSERT_EQ(path.dof(), 2); +} + +TEST_F(ConstructPiecewisePoly, CorrectPathInterval) { + toppra::Bound b = path.pathInterval(); + ASSERT_DOUBLE_EQ(b[0], 0); + ASSERT_DOUBLE_EQ(b[1], 2); +} + + + +// Current profile result (Release build) +// Took ~ 400 usec to evaluate 1000 points. +// scipy.PPoly took ~ 125 usec. +// Further improvements need not yield much added benefits. +TEST(ProfileEvaluationSpeed, Test1) { + static int dof{6}; + toppra::Matrix coeffs{4, dof}; + for (int i_col = 0; i_col < dof; i_col++) { + coeffs.col(i_col) << 2, 4, 5, 6; + } + toppra::Matrices coefficients{10, coeffs}; + toppra::Vector path_positions{1000}; + for (size_t i = 0; i < 1000; i++) { + path_positions(i) = std::min(0., std::max(10., (toppra::value_type)(i) / 100.0)); + } + std::chrono::steady_clock::time_point begin = + std::chrono::steady_clock::now(); + toppra::PiecewisePolyPath p = toppra::PiecewisePolyPath( + coefficients, + std::vector{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10}); + + p.eval(path_positions, 2); + std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); + std::cout << "Evaluation dones in: " + << std::chrono::duration_cast(end - + begin) + .count() + << " usec" << std::endl; +} + +class BadInputs : public testing::Test { +public: + BadInputs() : coeff(4, 2) { + coeff << 0, 0, 1, 1, 2, 2, 4, 4; + coefficents.push_back(coeff); + coefficents.push_back(coeff); + } + toppra::Matrix coeff; + toppra::Matrices coefficents; +}; + +TEST_F(BadInputs, ThrowIfBreakPointsNotIncreasing) { + ASSERT_THROW( + toppra::PiecewisePolyPath(coefficents, std::vector{0, 2, 1}), + std::runtime_error); +} + +TEST_F(BadInputs, ThrowIfWrongNumberOfBreakPoints) { + ASSERT_THROW( + toppra::PiecewisePolyPath(coefficents, std::vector{0, 1, 2, 3}), + std::runtime_error); +} + + +/* Python code to generate this test data + +import toppra as ta + +path = ta.SplineInterpolator([0, 1, 2, 3], [[0, 0], [1, 3], [2, 4], [0, 0]]) + +def print_cpp_code(p): + out = "" + for seg_idx in range(p.cspl.c.shape[1]): + out += "coeff{:d} << ".format(seg_idx) + for i, t in enumerate(p.cspl.c[:, seg_idx, :].flatten().tolist()): + if i == len(p.cspl.c[:, seg_idx, :].flatten().tolist()) - 1: + out += "{:f};\n".format(t) + else: + out += "{:f}, ".format(t) + return out + +print(print_cpp_code(path)) +print("breakpoints: {}".format([0, 1, 2, 3])) +x_eval = [0, 0.5, 1., 1.1, 2.5] +print("Eval for x_eval = {:}\npath(x_eval)=\n{}\npath(x_eval, 1)=\n{}\npath(x_eval, 2)=\n{}".format( + x_eval, path(x_eval), path(x_eval, 1), path(x_eval, 2))) + */ + +class CompareWithScipyCubicSpline : public testing::Test { + +public: + CompareWithScipyCubicSpline() { + toppra::Matrix coeff0{4, 2}, coeff1{4, 2}, coeff2{4, 2}; + coeff0 << -0.500000, -0.500000, 1.500000, 0.500000, 0.000000, 3.000000, 0.000000, 0.000000; + coeff1 << -0.500000, -0.500000, 0.000000, -1.000000, 1.500000, 2.500000, 1.000000, 3.000000; + coeff2 << -0.500000, -0.500000, -1.500000, -2.500000, 0.000000, -1.000000, 2.000000, 4.000000; + toppra::Matrices coefficents = {coeff0, coeff1, coeff2}; + path = toppra::PiecewisePolyPath(coefficents, std::vector{0, 1, 2, 3}); + + x_eval.resize(5); + x_eval << 0, 0.5, 1, 1.1, 2.5; + // Eval for x_eval = [0, 0.5, 1.0, 1.1, 2.5] + // path(x_eval)= + // [[0. 0. ] + // [0.3125 1.5625] + // [1. 3. ] + // [1.1495 3.2395] + // [1.5625 2.8125]] + toppra::Vector v0(2); v0 << 0, 0; + toppra::Vector v1(2); v1 << 0.3125, 1.5625; + toppra::Vector v2(2); v2 << 1. , 3. ; + toppra::Vector v3(2); v3 << 1.1495, 3.2395; + toppra::Vector v4(2); v4 << 1.5625, 2.8125; + expected_pos = toppra::Vectors{v0, v1, v2, v3, v4}; + + // Eval for x_eval = [0, 0.5, 1.0, 1.1, 2.5] + // path(x_eval, 1)= + // [[ 0. 3. ] + // [ 1.125 3.125] + // [ 1.5 2.5 ] + // [ 1.485 2.285] + // [-1.875 -3.875]] + v0 << 0. , 3. ; + v1 << 1.125, 3.125; + v2 << 1.5 , 2.5 ; + v3 << 1.485, 2.285; + v4 << -1.875, -3.875; + expected_vel = toppra::Vectors{v0, v1, v2, v3, v4}; + + // path(x_eval, 2)= + // [[ 3. 1. ] + // [ 1.5 -0.5] + // [ 0. -2. ] + // [-0.3 -2.3] + // [-4.5 -6.5]] + v0 << 3. , 1. ; + v1 << 1.5, -0.5; + v2 << 0. , -2. ; + v3 <<-0.3, -2.3; + v4 <<-4.5, -6.5; + expected_acc = toppra::Vectors{v0, v1, v2, v3, v4}; + } + + + toppra::Vector x_eval; + toppra::PiecewisePolyPath path; + toppra::Vectors expected_pos, expected_vel, expected_acc; +}; + + +TEST_F(CompareWithScipyCubicSpline, 0thDerivative){ + + auto res = path.eval(x_eval); + for(int i=0; i < 5; i++){ + ASSERT_TRUE(res[i].isApprox(expected_pos[i])) << "Comparing the " << i << "th" << res[i] << expected_pos[i]; + } +} + +TEST_F(CompareWithScipyCubicSpline, 1stDerivative){ + auto res = path.eval(x_eval, 1); + for(int i=0; i < 5; i++){ + ASSERT_TRUE(res[i].isApprox(expected_vel[i])) << "Comparing the " << i << "th" << res[i] << expected_vel[i]; + } +} + + +TEST_F(CompareWithScipyCubicSpline, 2stDerivative){ + auto res = path.eval(x_eval, 2); + for(int i=0; i < 5; i++){ + ASSERT_TRUE(res[i].isApprox(expected_acc[i])) << "Comparing the " << i << "th" << res[i] << "," << expected_acc[i]; + } +} + diff --git a/cpp/tests/test_solver.cpp b/cpp/tests/test_solver.cpp new file mode 100644 index 00000000..5a917975 --- /dev/null +++ b/cpp/tests/test_solver.cpp @@ -0,0 +1,140 @@ +#ifdef BUILD_WITH_qpOASES +#include +#endif +#ifdef BUILD_WITH_GLPK +#include +#endif + +#include +#include + +#include + +#include "gtest/gtest.h" + +class Solver : public testing::Test { +public: + Solver() : path(constructPath()) { + + // path has the equation: 0 * x ^ 3 + 1 * x ^ 2 + 2 x ^ 1 + 3 + } + + toppra::PiecewisePolyPath constructPath() { + toppra::Matrix coeff{4, 2}; + coeff.colwise() = toppra::Vector::LinSpaced(4, 0, 3); + toppra::Matrices coefficents = {coeff, coeff}; + toppra::PiecewisePolyPath p = + toppra::PiecewisePolyPath(coefficents, std::vector{0, 1, 2}); + return p; + } + + toppra::Vector getTimes (int N) + { + toppra::Bound I (path.pathInterval()); + return toppra::Vector::LinSpaced (N, I[0], I[1]); + } + + toppra::PiecewisePolyPath path; +}; + +std::map solutions; + +#ifdef BUILD_WITH_qpOASES +TEST_F(Solver, qpOASESWrapper) { + using namespace toppra; + int nDof = path.dof(); + Vector lb (-Vector::Ones(nDof)), + ub ( Vector::Ones(nDof)); + LinearConstraintPtr ljv (new constraint::LinearJointVelocity (lb, ub)); + LinearConstraintPtr lja (new constraint::LinearJointAcceleration (lb, ub)); + + int N = 10; + Vector times (getTimes(N)); + solver::qpOASESWrapper solver ({ ljv, lja }, path, times); + + EXPECT_EQ(solver.nbStages(), N-1); + EXPECT_EQ(solver.nbVars(), 2); + ASSERT_EQ(solver.deltas().size(), N-1); + for (int i = 0; i < N-1; ++i) + EXPECT_NEAR(solver.deltas()[i], times[i+1] - times[i], 1e-10); + + solver.setupSolver(); + Vector g (Vector::Ones(2)), solution; + Matrix H; + const value_type infty (std::numeric_limits::infinity()); + Bound x, xNext; + x << -infty, infty; + xNext << -infty, infty; + Vectors sols; + for (int i = 0; i < N; ++i) { + EXPECT_TRUE(solver.solveStagewiseOptim(i, H, g, x, xNext, solution)); + EXPECT_EQ(solution.size(), solver.nbVars()); + sols.emplace_back(solution); + } + solver.closeSolver(); + + solutions.emplace("qpOASES", sols); +} +#endif + +#ifdef BUILD_WITH_GLPK +TEST_F(Solver, GLPKWrapper) { + using namespace toppra; + int nDof = path.dof(); + Vector lb (-Vector::Ones(nDof)), + ub ( Vector::Ones(nDof)); + LinearConstraintPtr ljv (new constraint::LinearJointVelocity (lb, ub)); + LinearConstraintPtr lja (new constraint::LinearJointAcceleration (lb, ub)); + + int N = 10; + Vector times (getTimes(N)); + solver::GLPKWrapper solver ({ ljv, lja }, path, times); + + EXPECT_EQ(solver.nbStages(), N-1); + EXPECT_EQ(solver.nbVars(), 2); + ASSERT_EQ(solver.deltas().size(), N-1); + for (int i = 0; i < N-1; ++i) + EXPECT_NEAR(solver.deltas()[i], times[i+1] - times[i], 1e-10); + + solver.setupSolver(); + Vector g (Vector::Ones(2)), solution; + Matrix H; + const value_type infty (std::numeric_limits::infinity()); + Bound x, xNext; + x << -infty, infty; + xNext << -infty, infty; + Vectors sols; + for (int i = 0; i < N; ++i) { + EXPECT_TRUE(solver.solveStagewiseOptim(i, H, g, x, xNext, solution)); + EXPECT_EQ(solution.size(), solver.nbVars()); + sols.emplace_back(solution); + } + solver.closeSolver(); + + solutions.emplace("GLPK", sols); +} +#endif + +// Check that all the solvers returns the same solution. +// TODO each solver is expected to be tested on the same inputs. It should be +// templated, so that we know the same problem is setup (with a template hook to +// enable adding code specific to one solver). +TEST_F(Solver, consistency) +{ + auto ref = solutions.begin(); + bool first = true; + for(const auto& pair : solutions) { + if (first) { + first = false; + continue; + } + ASSERT_EQ(pair.second.size(), ref->second.size()); + for (int i = 0; i < pair.second.size(); ++i) { + ASSERT_EQ(pair.second[i].size(), ref->second[i].size()); + for (int j = 0; j < pair.second[i].size(); ++j) { + EXPECT_NEAR(pair.second[i][j], ref->second[i][j], 1e-6) + << " solvers " << ref->first << " and " << pair.first << " disagree."; + } + } + } +} diff --git a/docs/source/FAQs.rst b/docs/source/FAQs.rst deleted file mode 100644 index 548f04ef..00000000 --- a/docs/source/FAQs.rst +++ /dev/null @@ -1,45 +0,0 @@ -Frequently Asked Questions -====================================== - - -1. How many gridpoints should I take? ---------------------------------------- - -A very important parameter in solving path parameterization instances -with `toppra` is the number of the gridpoints. Below is how to create -an instance with 1000 uniform gridpoints. - - -.. code-block:: python - :linenos: - - gridpoints = np.linspace(0, path.duration, 1000) # 1000 points - instance = algo.TOPPRA([pc_vel, pc_acc], path, gridpoints=gridpoints) - - -Generally, more gridpoints give you better solution quality, but also -increase solution time. Often the increase in solution time is linear, -that is if it takes 5ms to solve an instance with 100 gridpoints, then -most likely `toppra` will take 10ms to solve another instance which -has 200 gridpoints. - -By default, `toppra` select 100 gridpoints. - -As a general rule of thumb, the number of gridpoints should be at -least twice the number of waypoints in the given path. - - -2. Minimum requirement on path smoothness -------------------------------------------------- - -TOPPRA requires the input path to be sufficiently smooth to work -properly. An example of a noisy path that will be very difficult to -work with can be seen below: - -.. image:: medias/faq_figure.png - -All toppra interpolators try to match all given waypoints, and hence -it can lead to large fluctuation if the waypoints change rapidly. In -this case, it is recommended to smooth the waypoints prior to using -toppra using for example `scipy.interpolation`. - diff --git a/docs/source/_static/toppra_illus.png b/docs/source/_static/toppra_illus.png new file mode 100644 index 00000000..67208a3a Binary files /dev/null and b/docs/source/_static/toppra_illus.png differ diff --git a/docs/source/conf.py b/docs/source/conf.py index 4dac13e0..4293b06c 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -41,9 +41,8 @@ 'sphinx.ext.napoleon', 'sphinx.ext.viewcode', 'sphinx.ext.intersphinx', - 'nbsphinx' -] - # Add mappings + 'nbsphinx'] + intersphinx_mapping = { 'urllib3': ('http://urllib3.readthedocs.org/en/latest', None), 'python': ('http://docs.python.org/3', None), @@ -95,7 +94,6 @@ # -- Options for HTML output ---------------------------------------------- - # The theme to use for HTML and HTML Help pages. See the documentation for # a list of builtin themes. # @@ -116,7 +114,7 @@ 'description': 'A robotic motion planning library for path-parametrization', 'fixed_sidebar': True, "sidebar_width": "270px", - "page_width": "1240px", + "page_width": "1040px", "show_related": True} # Add any paths that contain custom static files (such as style sheets) here, @@ -124,6 +122,7 @@ # so a file named "default.css" will overwrite the builtin "default.css". html_static_path = ['_static'] + # Custom sidebar templates, must be a dictionary that maps document names # to template names. # @@ -133,12 +132,12 @@ html_sidebars = { '**': [ 'about.html', - 'navigation.html', + 'localtoc.html', 'relations.html', # needs 'show_related': True theme option to display 'searchbox.html', ] } - +sidebar_collapse = True # -- Options for HTMLHelp output ------------------------------------------ @@ -190,6 +189,8 @@ # Grouping the document tree into Texinfo files. List of tuples # (source start file, target name, title, author, # dir menu entry, description, category) + + texinfo_documents = [ (master_doc, 'TOPP-RA', 'TOPP-RA Documentation', author, 'TOPP-RA', 'One line description of project.', @@ -198,3 +199,5 @@ +html_theme = 'alabaster' +html_static_path = ['_static'] diff --git a/docs/source/history.rst b/docs/source/history.rst new file mode 100644 index 00000000..1c9bc71f --- /dev/null +++ b/docs/source/history.rst @@ -0,0 +1,2 @@ + +.. include:: ../../HISTORY.md diff --git a/docs/source/index.rst b/docs/source/index.rst index 6c48a5e2..3ff69ba8 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -1,12 +1,7 @@ -.. TOPP-RA documentation master file, created by - sphinx-quick-start on Sat Nov 18 00:03:54 2017. - You can adapt this file completely to your liking, but it should at least - contain the root `toctree` directive. - +.. TOPP-RA documentation master file **TOPP-RA**: Path-parameterization for robots =================================================== - .. image:: https://circleci.com/gh/hungpham2511/toppra/tree/develop.svg?style=svg :target: https://circleci.com/gh/hungpham2511/toppra/tree/develop @@ -14,31 +9,62 @@ :target: https://toppra.readthedocs.io/en/latest/?badge=latest :alt: Documentation Status -**TOPP-RA** is a library for computing the time-optimal path -parametrization for robots subject to kinematic and dynamic -constraints. In general, given the inputs: -1. a geometric path `p(s)`, `s` in `[0, s_end]` ; +Overview +----------- + + +TOPP-RA is a library for computing path parametrizations for geometric +paths subject to certain forms of kinematic and dynamic constraints +Simply speaking given + +1. a smooth geometric path :math:`p(s), s \in [0, s_{end}]` ; 2. a list of constraints on joint velocity, joint accelerations, tool Cartesian velocity, et cetera. -**TOPP-RA** returns the time-optimal path parameterization: `s_dot -(s)`, from which the fastest trajectory `q(t)` that satisfies the -given constraints can be found. All of this is done in a few -milliseconds. - -Features ---------- +TOPP-RA can produce the time-optimal path parameterization +:math:`s_{dot} (s)`, from which the fastest trajectory `q(t)` that +satisfies the given constraints can be found. All of this is done in a +few milliseconds. There are some additional features that make it +easier to generate high-quality motions for robots: 1. Return the time-optimal parametrization or a parametrization with specified duration subject to constraints. 2. Able to handle multiple constraint types: - 1. joint torque, velocity and acceleration bounds; - 2. *robust* joint torque, velocity and acceleration bounds; - 3. Cartesian acceleration bounds; - 4. contact stability for legged robots. 3. Automatic grid-points selection. +.. figure:: _static/toppra_illus.png + + This is the time-optimal joint trajectory for a robot subject to + joint accelerations and velocities constraints computed using + TOPP-RA. See :ref:`tutorials` for more examples. + +User guide +-------------- + +.. toctree:: + :maxdepth: 1 + + installation + tutorials + notes + history + + +Citing TOPP-RA! +---------------- +If you find TOPP-RA useful and use it in your research, we encourage +you to + +1. reference the accompanying paper `«A new approach to Time-Optimal Path Parameterization based on Reachability Analysis» `_ *IEEE Transactions on Robotics*, vol. 34(3), pp. 645–659, 2018. +2. put a star on this repository! + + +Bug reports and supports +------------------------- +Please report any issues, questions via `Github issues tracker `_. + + Applications ------------ @@ -58,32 +84,4 @@ If you find this interesting, feel free to check out the paper: `_. This paper has been presented at ICRA 2019. -User Guide ----------- - -You can find on this page :ref:`installation`, :ref:`tutorials`, some -:ref:`notes` and :ref:`module_ref`. - -Citing TOPP-RA! ----------------- -If you find TOPP-RA useful and use it in your research, we encourage -you to - -1. reference the accompanying paper `«A new approach to Time-Optimal Path Parameterization based on Reachability Analysis» `_ *IEEE Transactions on Robotics*, vol. 34(3), pp. 645–659, 2018. -2. put a star on this repository! - - -Bug reports and supports -------------------------- -Please report any issues, questions via `Github issues tracker `_. - -.. toctree:: - :hidden: - :maxdepth: 3 - - installation - tutorials - notes - FAQs - modules diff --git a/docs/source/modules.rst b/docs/source/modules.rst deleted file mode 100644 index b16995f0..00000000 --- a/docs/source/modules.rst +++ /dev/null @@ -1,99 +0,0 @@ -.. _module_ref: - -Module references -========================= - -Interpolators -------------- - -Interpolator base class -^^^^^^^^^^^^^^^^^^^^^^^^^^^ -.. autoclass:: toppra.Interpolator - :members: - -Spline Interplator -^^^^^^^^^^^^^^^^^^^ -.. autoclass:: toppra.SplineInterpolator - :members: - -Rave Trajectory Wrapper -^^^^^^^^^^^^^^^^^^^^^^^^^^ -.. autoclass:: toppra.RaveTrajectoryWrapper - :members: - -Constraints ------------- - -Joint Acceleration Constraint -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -.. autoclass:: toppra.constraint.JointAccelerationConstraint - :members: - :special-members: - -Joint Velocity Constraint -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -.. autoclass:: toppra.constraint.JointVelocityConstraint - :members: - :special-members: - -Second Order Constraint -^^^^^^^^^^^^^^^^^^^^^^^^^ -.. autoclass:: toppra.constraint.SecondOrderConstraint - :members: - :special-members: - -Robust Linear Constraint -^^^^^^^^^^^^^^^^^^^^^^^^^^ -.. autoclass:: toppra.constraint.RobustLinearConstraint - :members: - -Canonical Linear Constraint (base class) -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -.. autoclass:: toppra.constraint.LinearConstraint - :members: - :special-members: - - -Constraints (base class) -^^^^^^^^^^^^^^^^^^^^^^^^^ -.. autoclass:: toppra.constraint.Constraint - -DiscretizationType (enum) -^^^^^^^^^^^^^^^^^^^^^^^^^^^ -.. autoclass:: toppra.constraint.DiscretizationType - :members: - -Algorithms ------------- - -TOPPRA (time-optimal) -^^^^^^^^^^^^^^^^^^^^^^ -.. autoclass:: toppra.algorithm.TOPPRA - :members: compute_parameterization, compute_trajectory, compute_feasible_sets, compute_controllable_sets - -TOPPRAsd (specific-duration) -^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -.. autoclass:: toppra.algorithm.TOPPRAsd - :members: set_desired_duration, compute_parameterization, compute_trajectory, compute_feasible_sets, compute_controllable_sets - -Solver Wrapper ----------------- - -All computations in TOPP-RA algorithms are done by the linear and -quadratic solvers, wrapped in solver wrappers. - -qpOASES (with hot-start) -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -.. autoclass:: toppra.solverwrapper.hotqpOASESSolverWrapper - :members: close_solver, setup_solver, solve_stagewise_optim - -seidel -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -.. autoclass:: toppra.solverwrapper.seidelWrapper - :members: solve_stagewise_optim - -ecos -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -.. autoclass:: toppra.solverwrapper.ecosWrapper - :members: solve_stagewise_optim - diff --git a/docs/source/notes.rst b/docs/source/notes.rst index 60299183..9c366d10 100644 --- a/docs/source/notes.rst +++ b/docs/source/notes.rst @@ -1,17 +1,20 @@ .. _notes: -Notes -======== + +Advanced usage +===================== .. _derivationKinematics: Derivation of kinematical quantities ------------------------------------ -In `toppra` we deal with geometric paths, which are often represented -as a function :math:`\mathbf p(s)`. Here :math:`s` is the path -position and usually belongs to the interval :math:`[0, 1]`. Notice -that `toppra` can also handle arbitrary interval. +In `toppra` we deal with geometric paths, which are mathematically +functions :math:`\mathbf p(s)`. Here :math:`s` is the path position +and usually belongs to the interval :math:`[0, 1]`. Notice that +`toppra` can also handle arbitrary interval. In the code a path is +represented by a child class inherited from the abstract +:class:`toppra.interpolator.AbstractGeometricPath`. Important expression relating kinematic quantities: @@ -21,3 +24,68 @@ Important expression relating kinematic quantities: \dot{\mathbf p}(t) &= \mathbf p'(s) \dot s(t) \\ \ddot{\mathbf p}(t) &= \mathbf p'(s) \ddot s(t) + \mathbf p''(s) \dot s(t)^2 +.. _module_ref: + +Path-parametrization Algorithms +-------------------------------- + +.. automodule:: toppra.algorithm + +TOPPRA (time-optimal) +^^^^^^^^^^^^^^^^^^^^^^ +.. autoclass:: toppra.algorithm.TOPPRA + :members: problem_data, compute_parameterization, compute_trajectory, compute_feasible_sets, compute_controllable_sets + +TOPPRAsd (specific-duration) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. autoclass:: toppra.algorithm.TOPPRAsd + :members: problem_data, set_desired_duration, compute_parameterization, compute_trajectory, compute_feasible_sets, compute_controllable_sets + + +Geometric paths +-------------------------------- + +.. automodule:: toppra.interpolator + +.. autoclass:: toppra.interpolator.AbstractGeometricPath + :members: __call__, dof, path_interval, waypoints + + +Spline Interplator +^^^^^^^^^^^^^^^^^^^ +.. autoclass:: toppra.SplineInterpolator + :members: __call__, dof, path_interval, waypoints + +Rave Trajectory Wrapper +^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. autoclass:: toppra.RaveTrajectoryWrapper + :members: __call__, dof, path_interval, waypoints + +.. autofunction:: toppra.interpolator.propose_gridpoints + +Constraints +-------------- + +.. automodule:: toppra.constraint + +Solver Wrapper +---------------- + +All computations in TOPP-RA algorithms are done by the linear and +quadratic solvers, wrapped in solver wrappers. + +qpOASES (with hot-start) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. autoclass:: toppra.solverwrapper.hotqpOASESSolverWrapper + :members: close_solver, setup_solver, solve_stagewise_optim + +seidel +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. autoclass:: toppra.solverwrapper.seidelWrapper + :members: solve_stagewise_optim + +ecos +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. autoclass:: toppra.solverwrapper.ecosWrapper + :members: solve_stagewise_optim + diff --git a/docs/source/tutorials.rst b/docs/source/tutorials.rst index e94f2e0b..95fa0257 100644 --- a/docs/source/tutorials.rst +++ b/docs/source/tutorials.rst @@ -1,6 +1,6 @@ .. _tutorials: -Tutorials +Quickstart ~~~~~~~~~~~~~~~~~~ Some tutorials to get you used to TOPP-RA @@ -13,3 +13,47 @@ Some tutorials to get you used to TOPP-RA tutorials/1_geometric_path tutorials/2_can_linear_constraints + +Frequently Asked Questions +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +1. How many gridpoints should I take? +--------------------------------------- + +A very important parameter in solving path parameterization instances +with `toppra` is the number of the gridpoints. Below is how to create +an instance with 1000 uniform gridpoints. + + +.. code-block:: python + :linenos: + + gridpoints = np.linspace(0, path.duration, 1000) # 1000 points + instance = algo.TOPPRA([pc_vel, pc_acc], path, gridpoints=gridpoints) + + +Generally, more gridpoints give you better solution quality, but also +increase solution time. Often the increase in solution time is linear, +that is if it takes 5ms to solve an instance with 100 gridpoints, then +most likely `toppra` will take 10ms to solve another instance which +has 200 gridpoints. + +By default, `toppra` select 100 gridpoints. + +As a general rule of thumb, the number of gridpoints should be at +least twice the number of waypoints in the given path. + + +2. Minimum requirement on path smoothness +------------------------------------------------- + +TOPPRA requires the input path to be sufficiently smooth to work +properly. An example of a noisy path that will be very difficult to +work with can be seen below: + +.. image:: medias/faq_figure.png + +All toppra interpolators try to match all given waypoints, and hence +it can lead to large fluctuation if the waypoints change rapidly. In +this case, it is recommended to smooth the waypoints prior to using +toppra using for example `scipy.interpolation`. diff --git a/docs/source/tutorials/0a_trapezoidal_scalar.rst b/docs/source/tutorials/0a_trapezoidal_scalar.rst deleted file mode 100644 index d17a1438..00000000 --- a/docs/source/tutorials/0a_trapezoidal_scalar.rst +++ /dev/null @@ -1,5 +0,0 @@ -Recreating the Trapezoidal Velocity Profile -================================================== - - -TODO diff --git a/docs/source/tutorials/1_geometric_path.rst b/docs/source/tutorials/1_geometric_path.rst index 6dd2c7a6..950b5409 100644 --- a/docs/source/tutorials/1_geometric_path.rst +++ b/docs/source/tutorials/1_geometric_path.rst @@ -1,3 +1,4 @@ + Path and Trajectory representation ===================================================== diff --git a/docs/source/tutorials/2_can_linear_constraints.rst b/docs/source/tutorials/2_can_linear_constraints.rst index aeb76e47..1e53e3c6 100644 --- a/docs/source/tutorials/2_can_linear_constraints.rst +++ b/docs/source/tutorials/2_can_linear_constraints.rst @@ -185,7 +185,7 @@ simple kinematic example, or any other situation. all_constraints = [pc_vel, pc_acc, pc_cart_acc] instance = algo.TOPPRA(all_constraints, path, solver_wrapper='seidel') - jnt_traj, _ = instance.compute_trajectory(0, 0) # resulting trajectory + jnt_traj = instance.compute_trajectory(0, 0) # resulting trajectory On my computer the whole process including evaluation of the dynamic coefficients, which is quite costly, takes 5-8 ms. Cartesian diff --git a/examples/cartesian_accel.py b/examples/cartesian_accel.py index ebf5cf2f..6b5c08d0 100644 --- a/examples/cartesian_accel.py +++ b/examples/cartesian_accel.py @@ -73,7 +73,7 @@ def g(q): # Retime the trajectory, only this step is necessary. t0 = time.time() - jnt_traj, _ = instance.compute_trajectory(0, 0) + jnt_traj = instance.compute_trajectory(0, 0) print("Parameterization time: {:} secs".format(time.time() - t0)) ts_sample = np.linspace(0, jnt_traj.get_duration(), 100) qs_sample = jnt_traj.eval(ts_sample) diff --git a/examples/kinematics.py b/examples/kinematics.py index 411b4226..ab712849 100644 --- a/examples/kinematics.py +++ b/examples/kinematics.py @@ -8,59 +8,63 @@ ta.setup_logging("INFO") -def generate_new_problem(): +def generate_new_problem(seed=9): # Parameters N_samples = 5 - SEED = 9 dof = 7 - np.random.seed(SEED) + np.random.seed(seed) way_pts = np.random.randn(N_samples, dof) - return way_pts + return ( + np.linspace(0, 1, 5), + way_pts, + 10 + np.random.rand(dof) * 20, + 10 + np.random.rand(dof) * 2, + ) def main(): - way_pts = generate_new_problem() - path = ta.SplineInterpolator(np.linspace(0, 1, 5), way_pts) - pc_vel = constraint.JointVelocityConstraint(10 + np.random.rand(path.dof) * 20) - pc_acc = constraint.JointAccelerationConstraint(10 + np.random.rand(path.dof) * 2) + ss, way_pts, vlims, alims = generate_new_problem() + path = ta.SplineInterpolator(ss, way_pts) + pc_vel = constraint.JointVelocityConstraint(vlims) + pc_acc = constraint.JointAccelerationConstraint(alims) # Setup a parametrization instance. The keyword arguments are optional. instance = algo.TOPPRA([pc_vel, pc_acc], path) # Retime the trajectory, only this step is necessary. t0 = time.time() - jnt_traj, _, data = instance.compute_trajectory(0, 0, return_data=True) + jnt_traj = instance.compute_trajectory() + # return_data flag outputs internal data obtained while computing # the paramterization. This include the time stamps corresponding # to the original waypoints. See below (line 53) to see how to # extract the time stamps. print("Parameterization time: {:} secs".format(time.time() - t0)) + instance.compute_feasible_sets() + ts_sample = np.linspace(0, jnt_traj.duration, 100) qs_sample = jnt_traj(ts_sample) for i in range(path.dof): # plot the i-th joint trajectory plt.plot(ts_sample, qs_sample[:, i], c="C{:d}".format(i)) # plot the i-th joint waypoints - plt.plot(data['t_waypts'], way_pts[:, i], 'x', c="C{:d}".format(i)) + plt.plot( + instance.problem_data["t_waypts"], way_pts[:, i], "x", c="C{:d}".format(i) + ) plt.xlabel("Time (s)") plt.ylabel("Joint position (rad/s^2)") plt.show() - # Compute the feasible sets and the controllable sets for viewing. - # Note that these steps are not necessary. - _, sd_vec, _ = instance.compute_parameterization(0, 0) - X = instance.compute_feasible_sets() - K = instance.compute_controllable_sets(0, 0) - X = np.sqrt(X) - K = np.sqrt(K) + K = instance.problem_data['K'] + X = instance.problem_data['X'] - plt.plot(X[:, 0], c='green', label="Feasible sets") - plt.plot(X[:, 1], c='green') - plt.plot(K[:, 0], '--', c='red', label="Controllable sets") - plt.plot(K[:, 1], '--', c='red') - plt.plot(sd_vec, label="Velocity profile") + plt.plot(X[:, 0], c="green", label="Feasible sets") + plt.plot(X[:, 1], c="green") + plt.plot(K[:, 0], "--", c="red", label="Controllable sets") + plt.plot(K[:, 1], "--", c="red") + plt.plot(instance.problem_data['sd'] ** 2, label="Velocity profile") plt.title("Path-position path-velocity plot") plt.xlabel("Path position") plt.ylabel("Path velocity square") @@ -68,5 +72,6 @@ def main(): plt.tight_layout() plt.show() -if __name__ == '__main__': + +if __name__ == "__main__": main() diff --git a/examples/kinematics_duration.py b/examples/kinematics_duration.py index da591890..c82f5bc1 100644 --- a/examples/kinematics_duration.py +++ b/examples/kinematics_duration.py @@ -37,7 +37,7 @@ def main(): instance.set_desired_duration(60) t0 = time.time() # Retime the trajectory, only this step is necessary. - jnt_traj, aux_traj = instance.compute_trajectory(0, 0) + jnt_traj = instance.compute_trajectory(0, 0) print("Parameterization took {:} secs".format(time.time() - t0)) ts_sample = np.linspace(0, jnt_traj.get_duration(), 100) qs_sample = jnt_traj.evaldd(ts_sample) diff --git a/examples/robust_kinematics.py b/examples/robust_kinematics.py index 60dbf862..17555524 100644 --- a/examples/robust_kinematics.py +++ b/examples/robust_kinematics.py @@ -7,15 +7,23 @@ def main(): - parser = argparse.ArgumentParser(description="An example showcasing the usage of robust constraints." - "A velocity constraint and a robust acceleration constraint" - "are considered in this script.") - parser.add_argument("-N", "--N", type=int, help="Number of segments in the discretization.", default=100) + parser = argparse.ArgumentParser( + description="An example showcasing the usage of robust constraints." + "A velocity constraint and a robust acceleration constraint" + "are considered in this script." + ) + parser.add_argument( + "-N", + "--N", + type=int, + help="Number of segments in the discretization.", + default=100, + ) parser.add_argument("-v", "--verbose", action="store_true", default=False) parser.add_argument("-du", "--du", default=1e-3, type=float) parser.add_argument("-dx", "--dx", default=5e-2, type=float) parser.add_argument("-dc", "--dc", default=9e-3, type=float) - parser.add_argument("-so", "--solver_wrapper", default='ecos') + parser.add_argument("-so", "--solver_wrapper", default="ecos") parser.add_argument("-i", "--interpolation_scheme", default=1, type=int) args = parser.parse_args() if args.verbose: @@ -41,12 +49,17 @@ def main(): path = ta.SplineInterpolator(np.linspace(0, 1, 5), way_pts) pc_vel = constraint.JointVelocityConstraint(vlim) pc_acc = constraint.JointAccelerationConstraint( - alim, discretization_scheme=constraint.DiscretizationType.Interpolation) + alim, discretization_scheme=constraint.DiscretizationType.Interpolation + ) robust_pc_acc = constraint.RobustLinearConstraint( - pc_acc, [args.du, args.dx, args.dc], args.interpolation_scheme) - instance = algo.TOPPRA([pc_vel, robust_pc_acc], path, - gridpoints=np.linspace(0, 1, args.N + 1), - solver_wrapper=args.solver_wrapper) + pc_acc, [args.du, args.dx, args.dc], args.interpolation_scheme + ) + instance = algo.TOPPRA( + [pc_vel, robust_pc_acc], + path, + gridpoints=np.linspace(0, 1, args.N + 1), + solver_wrapper=args.solver_wrapper, + ) X = instance.compute_feasible_sets() K = instance.compute_controllable_sets(0, 0) @@ -56,21 +69,22 @@ def main(): X = np.sqrt(X) K = np.sqrt(K) - plt.plot(X[:, 0], c='green', label="Feasible sets") - plt.plot(X[:, 1], c='green') - plt.plot(K[:, 0], '--', c='red', label="Controllable sets") - plt.plot(K[:, 1], '--', c='red') + plt.plot(X[:, 0], c="green", label="Feasible sets") + plt.plot(X[:, 1], c="green") + plt.plot(K[:, 0], "--", c="red", label="Controllable sets") + plt.plot(K[:, 1], "--", c="red") plt.plot(sd_vec, label="Velocity profile") plt.legend() plt.title("Path-position path-velocity plot") plt.show() - jnt_traj, aux_traj = instance.compute_trajectory(0, 0) + jnt_traj = instance.compute_trajectory(0, 0) ts_sample = np.linspace(0, jnt_traj.duration, 100) qs_sample = jnt_traj.evaldd(ts_sample) plt.plot(ts_sample, qs_sample) plt.show() -if __name__ == '__main__': + +if __name__ == "__main__": main() diff --git a/examples/scalar_example.py b/examples/scalar_example.py index 1e5da441..87690991 100644 --- a/examples/scalar_example.py +++ b/examples/scalar_example.py @@ -25,7 +25,7 @@ def main(): instance = algo.TOPPRA([pc_vel, pc_acc], path, solver_wrapper='seidel', gridpoints=np.linspace(0, 1.0, 1001)) - jnt_traj, aux_traj = instance.compute_trajectory(0, 0) + jnt_traj = instance.compute_trajectory(0, 0) duration = jnt_traj.duration print("Found optimal trajectory with duration {:f} sec".format(duration)) diff --git a/examples/torque_limit.py b/examples/torque_limit.py index a9842875..0fe2789f 100644 --- a/examples/torque_limit.py +++ b/examples/torque_limit.py @@ -74,7 +74,7 @@ def inv_dyn(q, qd, qdd): # Retime the trajectory, only this step is necessary. t0 = time.time() - jnt_traj, _ = instance.compute_trajectory(0, 0) + jnt_traj = instance.compute_trajectory(0, 0) print("Parameterization time: {:} secs".format(time.time() - t0)) ts_sample = np.linspace(0, jnt_traj.get_duration(), 100) qs_sample = jnt_traj.eval(ts_sample) diff --git a/mypy.ini b/mypy.ini new file mode 100644 index 00000000..ebcf395f --- /dev/null +++ b/mypy.ini @@ -0,0 +1,3 @@ +[mypy] +ignore_missing_imports = True + diff --git a/tasks.py b/tasks.py index f16f5941..ec40b0e0 100644 --- a/tasks.py +++ b/tasks.py @@ -1,26 +1,46 @@ """Collection of different operational tasts.""" from invoke import task + try: import pathlib2 as pathlib except ImportError: import pathlib +TOCHECK = [ + "toppra", +] + + +@task +def type_check(c): + """Check static typing.""" + for path in TOCHECK: + if not c.run("mypy {path}".format(path=path)): + return 1 + return 0 + @task def install_solvers(c, user=False): """Install backend solvers, e.g, qpoases.""" - install_dir = '/tmp/tox-qpoases' + install_dir = "/tmp/tox-qpoases" path = pathlib.Path(install_dir) if path.exists(): print("Path already exist") else: - c.run("git clone https://github.com/hungpham2511/qpOASES {}".format(install_dir)) + c.run( + "git clone https://github.com/hungpham2511/qpOASES {}".format(install_dir) + ) c.run("cd /tmp/tox-qpoases/ && mkdir bin && make") if user: flag = "--user" else: flag = "" - c.run("cd /tmp/tox-qpoases/interfaces/python/ && python setup.py install {}".format(flag)) + c.run( + "cd /tmp/tox-qpoases/interfaces/python/ && python setup.py install {}".format( + flag + ) + ) @task @@ -35,27 +55,39 @@ def make_venvs(c, python3=False, run_tests=False): flag = "--python python3" test_flag = "" - c.run("python -m virtualenv {flag} {venv_path} && \ + c.run( + "python -m virtualenv {flag} {venv_path} && \ {venv_path}/bin/pip install invoke pathlib numpy cython pytest".format( - venv_path=venv_path, flag=flag - )) - c.run(". {venv_path}/bin/activate && \ + venv_path=venv_path, flag=flag + ) + ) + c.run( + ". {venv_path}/bin/activate && \ invoke install-solvers && \ - pip install -e .[dev]".format(venv_path=venv_path)) + pip install -e .[dev]".format( + venv_path=venv_path + ) + ) if run_tests: - c.run("{test_flag} {venv_path}/bin/pytest -x".format(test_flag=test_flag, venv_path=venv_path)) + c.run( + "{test_flag} {venv_path}/bin/pytest -x".format( + test_flag=test_flag, venv_path=venv_path + ) + ) @task def lint(c, pycodestyle=False, pydocstyle=False): """Run linting on selected source files.""" - c.run("python -m pylint --rcfile=.pylintrc \ + c.run( + "python -m pylint --rcfile=.pylintrc \ tasks.py \ toppra/__init__.py \ toppra/utils.py \ toppra/interpolator.py \ toppra/exceptions.py \ - ") + " + ) # toppra/solverwrapper/solverwrapper.py if pycodestyle: c.run("pycodestyle toppra --max-line-length=120 --ignore=E731,W503,W605") @@ -72,20 +104,11 @@ def docker_build(c): @task def docker_start(c): """Start the development docker container.""" - c.run("docker run --rm --name toppra-dep -d \ + c.run( + "docker run --rm --name toppra-dep -d \ -v /home/hung/git/toppra:$HOME/toppra \ -e DISPLAY=unix$DISPLAY \ --net=host \ - hungpham2511/toppra-dep:0.0.3 sleep infinity") - - -@task -def docker_exec(c): - """Execute and link to a bash shell inside the stared docker container.""" - c.run("docker exec -it toppra-dep bash", pty=True) - - -@task -def docker_stop(c): - """Start the development docker container.""" - c.run("docker stop toppra-dep") + hungpham2511/toppra-dep:0.0.3 sleep infinity" + ) + print("Started new docker instance with name toppra-dep") diff --git a/tests/constraint/test_joint_acceleration.py b/tests/constraint/test_joint_acceleration.py index c5e3ea01..44e74164 100644 --- a/tests/constraint/test_joint_acceleration.py +++ b/tests/constraint/test_joint_acceleration.py @@ -77,8 +77,8 @@ def test_constraint_params(accel_constraint_setup): N = ss.shape[0] - 1 dof = path.dof - ps = path.evald(ss) - pss = path.evaldd(ss) + ps = path(ss, 1) + pss = path(ss, 2) F_actual = np.vstack((np.eye(dof), - np.eye(dof))) g_actual = np.hstack((alim[:, 1], - alim[:, 0])) diff --git a/tests/constraint/test_joint_velocity.py b/tests/constraint/test_joint_velocity.py index cc68f777..0a5ffc88 100644 --- a/tests/constraint/test_joint_velocity.py +++ b/tests/constraint/test_joint_velocity.py @@ -56,7 +56,7 @@ def test_constraint_satisfaction(self, velocity_pc_data): constraint_param = pc.compute_constraint_params(path, ss, 1.0) _, _, _, _, _, _, xlimit = constraint_param - qs = path.evald(ss) + qs = path(ss, 1) N = ss.shape[0] - 1 sd = cvx.Variable() @@ -107,7 +107,7 @@ def test_jnt_vel_varying_basic(): gridpoints = np.linspace(0, 2, 10) _, _, _, _, _, _, xlimit = constraint.compute_constraint_params(path, gridpoints, 1.0) # constraint splines - qs = path.evald(gridpoints) + qs = path(gridpoints, 1) # test sd = cvx.Variable() for i in range(ss_wpts.shape[0]): diff --git a/tests/constraint/test_second_order.py b/tests/constraint/test_second_order.py index 0ade6740..8a7875cd 100644 --- a/tests/constraint/test_second_order.py +++ b/tests/constraint/test_second_order.py @@ -59,9 +59,9 @@ def test_correctness(coefficients_functions): path, np.linspace(0, path.duration, 10), 1.0) # Correct params - q_vec = path.eval(np.linspace(0, path.duration, 10)) - qs_vec = path.evald(np.linspace(0, path.duration, 10)) - qss_vec = path.evaldd(np.linspace(0, path.duration, 10)) + q_vec = path(np.linspace(0, path.duration, 10)) + qs_vec = path(np.linspace(0, path.duration, 10), 1) + qss_vec = path(np.linspace(0, path.duration, 10), 2) for i in range(10): ai_ = A(q_vec[i]).dot(qs_vec[i]) @@ -99,9 +99,9 @@ def inv_dyn(q, qd, qdd): path, np.linspace(0, path.duration, 10), 1.0) # Correct params - p_vec = path.eval(np.linspace(0, path.duration, 10)) - ps_vec = path.evald(np.linspace(0, path.duration, 10)) - pss_vec = path.evaldd(np.linspace(0, path.duration, 10)) + p_vec = path(np.linspace(0, path.duration, 10)) + ps_vec = path(np.linspace(0, path.duration, 10), 1) + pss_vec = path(np.linspace(0, path.duration, 10), 2) dof = 2 F_actual = np.vstack((np.eye(dof), - np.eye(dof))) diff --git a/tests/interpolators/test_find_gridpoints.py b/tests/interpolators/test_find_gridpoints.py index 5a7a9f71..50d0646e 100644 --- a/tests/interpolators/test_find_gridpoints.py +++ b/tests/interpolators/test_find_gridpoints.py @@ -5,11 +5,10 @@ import pytest - @pytest.fixture(params=[[0, 1], [1.5, 2.7]]) def path(request): start, end = request.param - waypoints = [[0, 0.3, 0.5], [1, 2, 3], [0., 0.1, 0.2], [0, 0.5, 0]] + waypoints = [[0, 0.3, 0.5], [1, 2, 3], [0.0, 0.1, 0.2], [0, 0.5, 0]] ss = np.linspace(start, end, len(waypoints)) path = toppra.interpolator.SplineInterpolator(ss, waypoints) yield path, waypoints diff --git a/tests/interpolators/test_simple_path.py b/tests/interpolators/test_simple_path.py new file mode 100644 index 00000000..070307a5 --- /dev/null +++ b/tests/interpolators/test_simple_path.py @@ -0,0 +1,62 @@ +import toppra +import pytest +import numpy as np +from numpy.testing import assert_allclose + + +@pytest.fixture(name="f_scalar") +def give_a_simple_scalar_function_without_derivatives(): + f = toppra.SimplePath([0, 1, 2], np.array([0, 1, 1])) + yield f + + +def test_check_scalar_value_same_as_endpoint(f_scalar): + assert f_scalar(1) == 1.0 + assert f_scalar(2) == 1.0 + + +def test_first_derivative_is_continuous(f_scalar): + d1 = f_scalar(np.linspace(0, 2, 200), 1) + max_d1 = np.max(np.abs(np.diff(d1))) + assert max_d1 < 0.1 + + +def test_first_derivative_midpoint_is_average_between_two_points(f_scalar): + assert_allclose(f_scalar(1, 1), np.array(0.5)) + + +def test_first_derivative_zero_at_endpoints(f_scalar): + assert_allclose(f_scalar(0, 1), np.array(0)) + assert_allclose(f_scalar(2, 1), np.array(0)) + + +@pytest.fixture(name="f_scalar_wd") +def given_a_simple_scalar_function_with_first_derivatives(): + f = toppra.SimplePath([0, 1, 2], np.array([0, 1, 1]), np.array([0, 2, 0])) + yield f + + +def test_correct_derivatives_as_specified(f_scalar_wd): + assert_allclose(f_scalar_wd(1, 1), 2.0) + assert_allclose(f_scalar_wd(0, 1), 0) + assert_allclose(f_scalar_wd(2, 1), 0) + + +@pytest.fixture(name="fm") +def given_a_vector_path_without_specified_derivative(): + yield toppra.SimplePath([0, 1, 2], np.array([[0, 0], [1, 2], [1, 2]])) + + +def test_path_interpolate_are_correct(fm): + assert fm(0.5).shape == (2,) + assert isinstance(fm(0.5)[0], float) + assert_allclose(fm(0), [0, 0]) + assert_allclose(fm(1), [1, 2]) + + +def test_dof_is_correct(fm): + assert fm.dof == 2 + + +def test_path_interval_is_correct(fm): + assert_allclose(fm.path_interval, np.array([0.0, 2.0])) diff --git a/tests/interpolators/test_spline_interpolator.py b/tests/interpolators/test_spline_interpolator.py index 27cf2a19..480bc093 100644 --- a/tests/interpolators/test_spline_interpolator.py +++ b/tests/interpolators/test_spline_interpolator.py @@ -5,19 +5,33 @@ from ..testing_utils import IMPORT_OPENRAVEPY, IMPORT_OPENRAVEPY_MSG +@pytest.fixture(name='scalar_pi') +def given_scalar_spline_interpolator(): + sswp, wp, ss, path_interval = [[0, 0.3, 0.5], [1, 2, 3], [0.0, 0.1, 0.2, 0.3, 0.5], [0, 0.5]] + pi = SplineInterpolator(sswp, wp) # 1 + 2s + 3s^2 + yield pi, path_interval + + +def test_evaluate_single_value_is_scalar(scalar_pi): + pi, _ = scalar_pi + assert isinstance(pi(0), np.ndarray) -@pytest.mark.parametrize("sswp, wp, ss, path_interval", [ - [[0, 0.3, 0.5], [1, 2, 3], [0., 0.1, 0.2, 0.3, 0.5], [0, 0.5]], - [np.r_[0, 0.3, 0.5], [1, 2, 3], [0.], [0, 0.5]] -]) + +@pytest.mark.parametrize( + "sswp, wp, ss, path_interval", + [ + [[0, 0.3, 0.5], [1, 2, 3], [0.0, 0.1, 0.2, 0.3, 0.5], [0, 0.5]], + [np.r_[0, 0.3, 0.5], [1, 2, 3], [0.0], [0, 0.5]], + ], +) def test_scalar(sswp, wp, ss, path_interval): "A scalar (dof=1) trajectory." pi = SplineInterpolator(sswp, wp) # 1 + 2s + 3s^2 assert pi.dof == 1 - assert pi(ss).shape == (len(ss), ) - assert pi(ss, 1).shape == (len(ss), ) - assert pi(ss, 2).shape == (len(ss), ) + assert pi(ss).shape == (len(ss),) + assert pi(ss, 1).shape == (len(ss),) + assert pi(ss, 2).shape == (len(ss),) assert pi(0).shape == () npt.assert_allclose(pi.path_interval, path_interval) @@ -47,21 +61,22 @@ def test_1waypoints(): npt.assert_allclose(pi([0, 0], 1), [[0, 0, 0], [0, 0, 0]]) -@pytest.mark.parametrize("xs,ys, yd", [ - ([0, 1], [[0, 1], [2, 3]], [2, 2]), - ([0, 2], [[0, 1], [0, 3]], [0, 1]), -]) +@pytest.mark.parametrize( + "xs,ys, yd", + [([0, 1], [[0, 1], [2, 3]], [2, 2]), ([0, 2], [[0, 1], [0, 3]], [0, 1]),], +) def test_2waypoints(xs, ys, yd): "There is only two waypoints. Linear interpolation is done between them." - pi = SplineInterpolator(xs, ys, bc_type='natural') + pi = SplineInterpolator(xs, ys, bc_type="natural") npt.assert_allclose(pi.path_interval, xs) npt.assert_allclose(pi((xs[0] + xs[1]) / 2, 1), yd) npt.assert_allclose(pi(0, 2), np.zeros_like(ys[0])) -@pytest.fixture(scope='module') +@pytest.fixture(scope="module") def robot_fixture(rave_env): import openravepy as orpy + rave_env.Reset() rave_env.Load("data/lab1.env.xml") robot = rave_env.GetRobots()[0] @@ -70,25 +85,38 @@ def robot_fixture(rave_env): # Generate IKFast if needed iktype = orpy.IkParameterization.Type.Transform6D ikmodel = orpy.databases.inversekinematics.InverseKinematicsModel( - robot, iktype=iktype) + robot, iktype=iktype + ) if not ikmodel.load(): - print('Generating IKFast {0}. It will take few minutes...'.format( - iktype.name)) + print("Generating IKFast {0}. It will take few minutes...".format(iktype.name)) ikmodel.autogenerate() - print('IKFast {0} has been successfully generated'.format(iktype.name)) + print("IKFast {0} has been successfully generated".format(iktype.name)) yield robot @pytest.mark.skipif(not IMPORT_OPENRAVEPY, reason=IMPORT_OPENRAVEPY_MSG) -@pytest.mark.parametrize("ss_waypoints, waypoints", [ - [[0, 0.2, 0.5, 0.9], [[0.377, -0.369, 1.042, -0.265, -0.35, -0.105, -0.74], - [1.131, 0.025, 0.778, 0.781, 0.543, -0.139, 0.222], - [-1.055, 1.721, -0.452, -0.268, 0.182, -0.935, 2.257], - [-0.274, -0.164, 1.492, 1.161, 1.958, -1.125, 0.567]]], - [[0, 0.2], [[0.377, -0.369, 1.042, -0.265, -0.35, -0.105, -0.74], - [1.131, 0.025, 0.778, 0.781, 0.543, -0.139, 0.222]]], - [[0], [[0.377, -0.369, 1.042, -0.265, -0.35, -0.105, -0.74]]] -]) +@pytest.mark.parametrize( + "ss_waypoints, waypoints", + [ + [ + [0, 0.2, 0.5, 0.9], + [ + [0.377, -0.369, 1.042, -0.265, -0.35, -0.105, -0.74], + [1.131, 0.025, 0.778, 0.781, 0.543, -0.139, 0.222], + [-1.055, 1.721, -0.452, -0.268, 0.182, -0.935, 2.257], + [-0.274, -0.164, 1.492, 1.161, 1.958, -1.125, 0.567], + ], + ], + [ + [0, 0.2], + [ + [0.377, -0.369, 1.042, -0.265, -0.35, -0.105, -0.74], + [1.131, 0.025, 0.778, 0.781, 0.543, -0.139, 0.222], + ], + ], + [[0], [[0.377, -0.369, 1.042, -0.265, -0.35, -0.105, -0.74]]], + ], +) def test_compute_rave_trajectory(robot_fixture, ss_waypoints, waypoints): """From the given spline trajectory, compute openrave trajectory.""" active_indices = robot_fixture.GetActiveDOFIndices() @@ -109,12 +137,11 @@ def test_compute_rave_trajectory(robot_fixture, ss_waypoints, waypoints): qdds_rave = [] for t in xs: data = traj.Sample(t) - qs_rave.append(spec.ExtractJointValues( - data, robot_fixture, active_indices, 0)) - qds_rave.append(spec.ExtractJointValues( - data, robot_fixture, active_indices, 1)) - qdds_rave.append(spec.ExtractJointValues( - data, robot_fixture, active_indices, 2)) + qs_rave.append(spec.ExtractJointValues(data, robot_fixture, active_indices, 0)) + qds_rave.append(spec.ExtractJointValues(data, robot_fixture, active_indices, 1)) + qdds_rave.append( + spec.ExtractJointValues(data, robot_fixture, active_indices, 2) + ) # Assert all close npt.assert_allclose(qs_spline, qs_rave) diff --git a/tests/retime/robustness/test_robustness_main.py b/tests/retime/robustness/test_robustness_main.py index 2a84f3d0..eb63842f 100644 --- a/tests/retime/robustness/test_robustness_main.py +++ b/tests/retime/robustness/test_robustness_main.py @@ -27,7 +27,7 @@ def test_robustness_main(request): parsed_problems = [] path = pathlib.Path(__file__) path = path / '../problem_suite_1.yaml' - problem_dict = yaml.load(path.resolve().read_text()) + problem_dict = yaml.load(path.resolve().read_text(), Loader=yaml.SafeLoader) for key in problem_dict: if len(problem_dict[key]['ss_waypoints']) == 2: ss_waypoints = np.linspace(problem_dict[key]['ss_waypoints'][0], @@ -76,7 +76,8 @@ def test_robustness_main(request): instance.set_desired_duration(problem_data['desired_duration']) t2 = time.time() - jnt_traj, aux_traj, data = instance.compute_trajectory(0, 0, return_data=True) + jnt_traj = instance.compute_trajectory(0, 0) + data = instance.problem_data t3 = time.time() if visualize: diff --git a/tests/retime/test_correct_velocity.py b/tests/retime/test_correct_velocity.py index b7688de3..9018d6de 100644 --- a/tests/retime/test_correct_velocity.py +++ b/tests/retime/test_correct_velocity.py @@ -4,8 +4,8 @@ import toppra.constraint as constraint -@pytest.fixture(params=["spline", "poly"]) -def basic_path(request): +@pytest.fixture(params=["spline", "poly"], name="basic_path") +def given_basic_path(request): """ Return a generic path. """ if request.param == "spline": @@ -18,13 +18,18 @@ def basic_path(request): yield path +@pytest.fixture(name='constraints') +def given_simple_constraints(basic_constraints): + vel_c, acc_c, ro_acc_c = basic_constraints + yield [vel_c, acc_c] + + @pytest.mark.parametrize("solver_wrapper", ["hotqpoases", "seidel"]) -def test_zero_velocity(basic_constraints, basic_path, solver_wrapper): +def test_zero_velocity(constraints, basic_path, solver_wrapper): """Check that initial and final velocity are correct.""" - vel_c, acc_c, ro_acc_c = basic_constraints - instance = toppra.algorithm.TOPPRA([vel_c, acc_c], basic_path, + instance = toppra.algorithm.TOPPRA(constraints, basic_path, solver_wrapper=solver_wrapper) - jnt_traj, _ = instance.compute_trajectory(0, 0) + jnt_traj = instance.compute_trajectory(0, 0) # assertion initial_velocity = jnt_traj(0, 1) @@ -36,11 +41,10 @@ def test_zero_velocity(basic_constraints, basic_path, solver_wrapper): @pytest.mark.parametrize("velocity_start", [0, 0.1]) @pytest.mark.parametrize("velocity_end", [0, 0.1]) @pytest.mark.parametrize("solver_wrapper", ["hotqpoases", "seidel"]) -def test_nonzero_velocity(velocity_start, velocity_end, basic_constraints, basic_path, solver_wrapper): - vel_c, acc_c, ro_acc_c = basic_constraints - instance = toppra.algorithm.TOPPRA([vel_c, acc_c], basic_path, +def test_nonzero_velocity(velocity_start, velocity_end, constraints, basic_path, solver_wrapper): + instance = toppra.algorithm.TOPPRA(constraints, basic_path, solver_wrapper=solver_wrapper) - jnt_traj, _ = instance.compute_trajectory(velocity_start, velocity_end) + jnt_traj = instance.compute_trajectory(velocity_start, velocity_end) # assertion initial_velocity = jnt_traj(0, 1) @@ -54,11 +58,10 @@ def test_nonzero_velocity(velocity_start, velocity_end, basic_constraints, basic @pytest.mark.parametrize("solver_wrapper", ["hotqpoases", "seidel"]) @pytest.mark.parametrize("velocities", [[0, -1], [-1, 0], [-1, -1]]) -def test_invalid_velocity(velocities, basic_constraints, basic_path, solver_wrapper): - vel_c, acc_c, ro_acc_c = basic_constraints - instance = toppra.algorithm.TOPPRA([vel_c, acc_c], basic_path, +def test_invalid_velocity(velocities, constraints, basic_path, solver_wrapper): + instance = toppra.algorithm.TOPPRA(constraints, basic_path, solver_wrapper=solver_wrapper) - with pytest.raises(ValueError) as err: - jnt_traj, _ = instance.compute_trajectory(*velocities) + with pytest.raises(toppra.exceptions.BadInputVelocities) as err: + jnt_traj = instance.compute_trajectory(*velocities) assert "Negative" in err.value.args[0] diff --git a/tests/retime/test_retime_basic.py b/tests/retime/test_retime_basic.py index 5782e8eb..4772988a 100644 --- a/tests/retime/test_retime_basic.py +++ b/tests/retime/test_retime_basic.py @@ -13,7 +13,7 @@ @pytest.mark.parametrize("solver_wrapper", ["qpoases", "hotqpoases", "seidel"]) -def test_toppra_linear(basic_constraints, basic_path, solver_wrapper): +def test_simple_set_operations(basic_constraints, basic_path, solver_wrapper): """Solve some basic problem instances. Passing this test guaranetees that the basic functionalities are @@ -30,7 +30,7 @@ def test_toppra_linear(basic_constraints, basic_path, solver_wrapper): assert np.all(K >= 0) assert not np.any(np.isnan(K)) - traj, _ = instance.compute_trajectory(0, 0) + traj = instance.compute_trajectory(0, 0) assert traj is not None diff --git a/tests/retime/test_retime_scaling.py b/tests/retime/test_retime_scaling.py index 0299bc56..64d06f4d 100644 --- a/tests/retime/test_retime_scaling.py +++ b/tests/retime/test_retime_scaling.py @@ -10,6 +10,7 @@ toppra.setup_logging(level="INFO") +@pytest.mark.skip(reason="Scaling is no longer supported") @pytest.mark.parametrize("solver_wrapper", ["hotqpoases", "seidel"]) def test_linear_case1(basic_constraints, basic_path, solver_wrapper): """A generic test case. @@ -23,7 +24,7 @@ def test_linear_case1(basic_constraints, basic_path, solver_wrapper): [vel_c, acc_c], basic_path, solver_wrapper=solver_wrapper, scaling=1.0) instance_scale05 = toppra.algorithm.TOPPRA( [vel_c, acc_c], basic_path, solver_wrapper=solver_wrapper, scaling=5) - traj1, _ = instance_scale1.compute_trajectory() - traj05, _ = instance_scale05.compute_trajectory() + traj1 = instance_scale1.compute_trajectory() + traj05 = instance_scale05.compute_trajectory() # accurate up to 0.1% np.testing.assert_allclose(traj1.duration, traj05.duration, rtol=1e-3) diff --git a/tests/retime/test_retime_wconic_constraints.py b/tests/retime/test_retime_wconic_constraints.py index 49cb4f2a..37e9fc13 100644 --- a/tests/retime/test_retime_wconic_constraints.py +++ b/tests/retime/test_retime_wconic_constraints.py @@ -43,6 +43,6 @@ def test_toppra_conic(vel_accel_robustaccel, path, solver_wrapper): assert np.all(K >= 0) assert not np.any(np.isnan(K)) - traj, _ = ro_instance.compute_trajectory(0, 0) + traj = ro_instance.compute_trajectory(0, 0) assert traj is not None assert traj.duration < 20 and traj.duration > 0 diff --git a/toppra/__init__.py b/toppra/__init__.py index 2f16e42b..fc3ea317 100644 --- a/toppra/__init__.py +++ b/toppra/__init__.py @@ -7,15 +7,20 @@ using the algorithm `TOPP-RA`. """ import logging + +# core modules from .interpolator import RaveTrajectoryWrapper, SplineInterpolator,\ UnivariateSplineInterpolator, PolynomialPath -from .utils import smooth_singularities, setup_logging -from .planning_utils import retime_active_joints_kinematics,\ - create_rave_torque_path_constraint +from .simplepath import SimplePath from . import constraint from . import algorithm from . import solverwrapper +# utility +from .utils import smooth_singularities, setup_logging +from .planning_utils import retime_active_joints_kinematics,\ + create_rave_torque_path_constraint + # set nullhandler by default logging.getLogger('toppra').addHandler(logging.NullHandler()) diff --git a/toppra/algorithm/__init__.py b/toppra/algorithm/__init__.py index 696c250e..47da59b8 100644 --- a/toppra/algorithm/__init__.py +++ b/toppra/algorithm/__init__.py @@ -1 +1,5 @@ +"""TOPP-RA implements multiple path-parametrization algorithms. +""" from .reachabilitybased import TOPPRA, TOPPRAsd + +__all__ = ["TOPPRA", "TOPPRAsd"] diff --git a/toppra/algorithm/algorithm.py b/toppra/algorithm/algorithm.py index e147ed6f..37fbcc29 100644 --- a/toppra/algorithm/algorithm.py +++ b/toppra/algorithm/algorithm.py @@ -4,16 +4,11 @@ from ..constants import TINY from toppra.interpolator import SplineInterpolator, AbstractGeometricPath +import toppra.interpolator as interpolator import logging -logger = logging.getLogger(__name__) -try: - import openravepy as orpy -except ImportError as err: - logger.debug("Unable to import openravepy. Exception: %s" % err.args[0]) -except SyntaxError as err: - logger.debug("Unable to import openravepy. Exception: %s" % err.args[0]) +logger = logging.getLogger(__name__) class ParameterizationAlgorithm(object): @@ -31,9 +26,34 @@ class ParameterizationAlgorithm(object): gridpoints: array, optional If not given, automatically generate a grid with 100 steps. """ + def __init__(self, constraint_list, path, gridpoints=None): self.constraints = constraint_list # Attr self.path = path # Attr + self._problem_data = {} + # Handle gridpoints + if gridpoints is None: + gridpoints = interpolator.propose_gridpoints(path, max_err_threshold=1e-3) + logger.info( + "No gridpoint specified. Automatically choose a gridpoint. See `propose_gridpoints`." + ) + + if ( + path.path_interval[0] != gridpoints[0] + or path.path_interval[1] != gridpoints[-1] + ): + raise ValueError("Invalid manually supplied gridpoints.") + self.gridpoints = np.array(gridpoints) + self._N = len(gridpoints) - 1 # Number of stages. Number of point is _N + 1 + for i in range(self._N): + if gridpoints[i + 1] <= gridpoints[i]: + logger.fatal("Input gridpoints are not monotonically increasing.") + raise ValueError("Bad input gridpoints.") + + @property + def problem_data(self): + """Dict[str, Any]: Intermediate data obtained while solving the problem.""" + return self._problem_data def compute_parameterization(self, sd_start, sd_end): """Compute a path parameterization. @@ -66,7 +86,7 @@ def compute_parameterization(self, sd_start, sd_end): """ raise NotImplementedError - def compute_trajectory(self, sd_start=0, sd_end=0, return_profile=False, bc_type=None, return_data=False): + def compute_trajectory(self, sd_start=0, sd_end=0, return_data=False): """Compute the resulting joint trajectory and auxilliary trajectory. If parameterization fails, return a tuple of None(s). @@ -77,15 +97,8 @@ def compute_trajectory(self, sd_start=0, sd_end=0, return_profile=False, bc_type Starting path velocity. sd_end: float Goal path velocity. - return_profile: bool, optional - If true, return a tuple containing data. NOTE: This - function is obsolete, use return_data instead. return_data: bool, optional If true, return a dict containing the internal data. - bc_type: str, optional - Boundary condition for the resulting trajectory. Can be - 'not-a-knot', 'clamped', 'natural' or 'periodic'. See - scipy.CubicSpline documentation for more details. Returns ------- @@ -96,21 +109,15 @@ def compute_trajectory(self, sd_start=0, sd_end=0, return_profile=False, bc_type Time-parameterized auxiliary variable trajectory. If unable to parameterize or if there is no auxiliary variable, return None. - profiles: tuple - Return if return_profile is True, results from - compute_parameterization. - data: dict - Return if return_data is True. """ - sdd_grid, sd_grid, v_grid, K = self.compute_parameterization(sd_start, sd_end, return_data=True) + sdd_grid, sd_grid, v_grid, K = self.compute_parameterization( + sd_start, sd_end, return_data=True + ) # fail condition: sd_grid is None, or there is nan in sd_grid if sd_grid is None or np.isnan(sd_grid).any(): - if return_profile or return_data: - return None, None, (sdd_grid, sd_grid, v_grid, K) - else: - return None, None + return None, None # Gridpoint time instances t_grid = np.zeros(self._N + 1) @@ -128,10 +135,16 @@ def compute_trajectory(self, sd_start=0, sd_end=0, return_profile=False, bc_type t_grid = np.delete(t_grid, skip_ent) scaling = self.gridpoints[-1] / self.path.duration gridpoints = np.delete(self.gridpoints, skip_ent) / scaling - q_grid = self.path.eval(gridpoints) + q_grid = self.path(gridpoints) - traj_spline = SplineInterpolator(t_grid, q_grid, ( - (1, self.path(0, 1) * sd_start), (1, self.path(self.path.duration, 1) * sd_end))) + traj_spline = SplineInterpolator( + t_grid, + q_grid, + ( + (1, self.path(0, 1) * sd_start), + (1, self.path(self.path.duration, 1) * sd_end), + ), + ) if v_grid.shape[1] == 0: v_spline = None @@ -142,13 +155,11 @@ def compute_trajectory(self, sd_start=0, sd_end=0, return_profile=False, bc_type v_grid_ = np.delete(v_grid_, skip_ent, axis=0) v_spline = SplineInterpolator(t_grid, v_grid_) - if return_profile: - return traj_spline, v_spline, (sdd_grid, sd_grid, v_grid, K) - elif return_data: - # NOTE: the time stamps for each (original) waypoint are - # evaluated by interpolating the grid points. - t_waypts = np.interp(self.path.get_waypoints()[0], gridpoints, t_grid) - return traj_spline, v_spline, {'sdd': sdd_grid, 'sd': sd_grid, - 'v': v_grid, 'K': K, 't_waypts': t_waypts} - else: - return traj_spline, v_spline + self._problem_data.update( + {"sdd": sdd_grid, "sd": sd_grid, "v": v_grid, "K": K, "v_traj": v_spline} + ) + if self.path.waypoints is not None: + t_waypts = np.interp(self.path.waypoints[0], gridpoints, t_grid) + self._problem_data.update({"t_waypts": t_waypts}) + + return traj_spline diff --git a/toppra/algorithm/reachabilitybased/reachability_algorithm.py b/toppra/algorithm/reachabilitybased/reachability_algorithm.py index 6363af37..d4206f6a 100644 --- a/toppra/algorithm/reachabilitybased/reachability_algorithm.py +++ b/toppra/algorithm/reachabilitybased/reachability_algorithm.py @@ -7,6 +7,7 @@ import numpy as np import logging + logger = logging.getLogger(__name__) @@ -55,22 +56,13 @@ class ReachabilityAlgorithm(ParameterizationAlgorithm): - compute_reachable_sets - compute_feasible_sets """ - def __init__(self, constraint_list, path, gridpoints=None, solver_wrapper=None, scaling=1): - super(ReachabilityAlgorithm, self).__init__(constraint_list, path, gridpoints=gridpoints) - - # Handle gridpoints - if gridpoints is None: - gridpoints = interpolator.propose_gridpoints(path, max_err_threshold=1e-3) - logger.info("No gridpoint specified. Automatically choose a gridpoint. See `propose_gridpoints`.") - - if path.path_interval[0] != gridpoints[0] or path.path_interval[1] != gridpoints[-1]: - raise ValueError("Invalid manually supplied gridpoints.") - self.gridpoints = np.array(gridpoints) - self._N = len(gridpoints) - 1 # Number of stages. Number of point is _N + 1 - for i in range(self._N): - if gridpoints[i + 1] <= gridpoints[i]: - logger.fatal("Input gridpoints are not monotonically increasing.") - raise ValueError("Bad input gridpoints.") + + def __init__( + self, constraint_list, path, gridpoints=None, solver_wrapper=None, scaling=1 + ): + super(ReachabilityAlgorithm, self).__init__( + constraint_list, path, gridpoints=gridpoints + ) # path scaling (for numerical stability) if scaling < 0: # automatic scaling factor selection @@ -78,7 +70,9 @@ def __init__(self, constraint_list, path, gridpoints=None, solver_wrapper=None, qs_sam = path(np.linspace(0, 1, 5) * path.duration, 1) qs_average = np.sum(np.abs(qs_sam)) / path.dof / 5 scaling = np.sqrt(qs_average) - logger.info("[auto-scaling] Average path derivative: {:}".format(qs_average)) + logger.info( + "[auto-scaling] Average path derivative: {:}".format(qs_average) + ) logger.info("[auto-scaling] Selected scaling factor: {:}".format(scaling)) else: logger.info("Scaling factor: {:f}".format(scaling)) @@ -98,9 +92,11 @@ def __init__(self, constraint_list, path, gridpoints=None, solver_wrapper=None, # Select solver wrapper automatically available_solvers = toppra.solverwrapper.available_solvers() if solver_wrapper is None: - logger.info("Solver wrapper not supplied. Choose solver wrapper automatically!") + logger.info( + "Solver wrapper not supplied. Choose solver wrapper automatically!" + ) if has_conic: - if not available_solvers['ecos']: + if not available_solvers["ecos"]: raise exceptions.ToppraError("Solverwrapper not available.") solver_wrapper = "ecos" else: @@ -110,29 +106,57 @@ def __init__(self, constraint_list, path, gridpoints=None, solver_wrapper=None, # Check solver-wrapper suitability if has_conic: - assert solver_wrapper.lower() in ['cvxpy', 'ecos'], \ - "Problem has conic constraints, solver {:} is not suitable".format(solver_wrapper) + assert solver_wrapper.lower() in [ + "cvxpy", + "ecos", + ], "Problem has conic constraints, solver {:} is not suitable".format( + solver_wrapper + ) else: - assert solver_wrapper.lower() in ['cvxpy', 'qpoases', 'ecos', 'hotqpoases', 'seidel'], \ - "Solver {:} not found".format(solver_wrapper) + assert solver_wrapper.lower() in [ + "cvxpy", + "qpoases", + "ecos", + "hotqpoases", + "seidel", + ], "Solver {:} not found".format(solver_wrapper) if solver_wrapper.lower() == "cvxpy": from toppra.solverwrapper.cvxpy_solverwrapper import cvxpyWrapper - self.solver_wrapper = cvxpyWrapper(self.constraints, self.path, self.gridpoints) + + self.solver_wrapper = cvxpyWrapper( + self.constraints, self.path, self.gridpoints + ) elif solver_wrapper.lower() == "qpoases": from toppra.solverwrapper.qpoases_solverwrapper import qpOASESSolverWrapper - self.solver_wrapper = qpOASESSolverWrapper(self.constraints, self.path, self.gridpoints) + + self.solver_wrapper = qpOASESSolverWrapper( + self.constraints, self.path, self.gridpoints + ) elif solver_wrapper.lower() == "hotqpoases": - from toppra.solverwrapper.hot_qpoases_solverwrapper import hotqpOASESSolverWrapper - self.solver_wrapper = hotqpOASESSolverWrapper(self.constraints, self.path, self.gridpoints) + from toppra.solverwrapper.hot_qpoases_solverwrapper import ( + hotqpOASESSolverWrapper, + ) + + self.solver_wrapper = hotqpOASESSolverWrapper( + self.constraints, self.path, self.gridpoints + ) elif solver_wrapper.lower() == "ecos": from toppra.solverwrapper.ecos_solverwrapper import ecosWrapper - self.solver_wrapper = ecosWrapper(self.constraints, self.path, self.gridpoints) + + self.solver_wrapper = ecosWrapper( + self.constraints, self.path, self.gridpoints + ) elif solver_wrapper.lower() == "seidel": from toppra.solverwrapper.cy_seidel_solverwrapper import seidelWrapper - self.solver_wrapper = seidelWrapper(self.constraints, self.path, self.gridpoints) + + self.solver_wrapper = seidelWrapper( + self.constraints, self.path, self.gridpoints + ) else: - raise NotImplementedError("Solver wrapper {:} not found!".format(solver_wrapper)) + raise NotImplementedError( + "Solver wrapper {:} not found!".format(solver_wrapper) + ) def compute_feasible_sets(self): """Compute the sets of feasible squared velocities. @@ -155,15 +179,18 @@ def compute_feasible_sets(self): self.solver_wrapper.setup_solver() for i in range(self._N + 1): X[i, 0] = self.solver_wrapper.solve_stagewise_optim( - i, Hzero, g_lower, -CVXPY_MAXX, CVXPY_MAXX, -CVXPY_MAXX, CVXPY_MAXX)[1] + i, Hzero, g_lower, -CVXPY_MAXX, CVXPY_MAXX, -CVXPY_MAXX, CVXPY_MAXX + )[1] X[i, 1] = self.solver_wrapper.solve_stagewise_optim( - i, Hzero, -g_lower, -CVXPY_MAXX, CVXPY_MAXX, -CVXPY_MAXX, CVXPY_MAXX)[1] + i, Hzero, -g_lower, -CVXPY_MAXX, CVXPY_MAXX, -CVXPY_MAXX, CVXPY_MAXX + )[1] if logger.getEffectiveLevel() == logging.DEBUG: logger.debug("X[{:d}]={:}".format(i, X[i])) self.solver_wrapper.close_solver() for i in range(self._N + 1): if X[i, 0] < 0: X[i, 0] = 0 + self._problem_data.update({'X': X}) return X def compute_controllable_sets(self, sdmin, sdmax): @@ -193,8 +220,10 @@ def compute_controllable_sets(self, sdmin, sdmax): if K[i, 0] < 0: K[i, 0] = 0 if np.isnan(K[i]).any(): - logger.warning("A numerical error occurs: The controllable set at step " - "[{:d} / {:d}] can't be computed.".format(i, self._N + 1)) + logger.warning( + "A numerical error occurs: The controllable set at step " + "[{:d} / {:d}] can't be computed.".format(i, self._N + 1) + ) return K if logger.isEnabledFor(logging.DEBUG): logger.debug("[Compute controllable sets] K_{:d}={:}".format(i, K[i])) @@ -228,12 +257,14 @@ def _one_step(self, i, K_next): nV = self.solver_wrapper.get_no_vars() g_upper = np.zeros(nV) g_upper[0] = 1e-9 - g_upper[1] = - 1 + g_upper[1] = -1 x_upper = self.solver_wrapper.solve_stagewise_optim( - i, None, g_upper, np.nan, np.nan, K_next[0], K_next[1])[1] + i, None, g_upper, np.nan, np.nan, K_next[0], K_next[1] + )[1] # import ipdb; ipdb.set_trace() x_lower = self.solver_wrapper.solve_stagewise_optim( - i, None, - g_upper, np.nan, np.nan, K_next[0], K_next[1])[1] + i, None, -g_upper, np.nan, np.nan, K_next[0], K_next[1] + )[1] res[:] = [x_lower, x_upper] return res @@ -270,11 +301,16 @@ def compute_parameterization(self, sd_start, sd_end, return_data=False): `return_data` is True. """ if sd_end < 0 or sd_start < 0: - raise ValueError("Negative path velocities: path velocities must be positive: (%s, %s)" % (sd_start, sd_end)) + raise exceptions.BadInputVelocities( + "Negative path velocities: path velocities must be positive: (%s, %s)" + % (sd_start, sd_end) + ) K = self.compute_controllable_sets(sd_end, sd_end) if np.isnan(K).any(): - logger.warning("An error occurred when computing controllable velocities. " - "The path is not controllable, or is badly conditioned.") + logger.warning( + "An error occurred when computing controllable velocities. " + "The path is not controllable, or is badly conditioned." + ) if return_data: return None, None, None, K else: @@ -282,9 +318,11 @@ def compute_parameterization(self, sd_start, sd_end, return_data=False): x_start = sd_start ** 2 if x_start + SMALL < K[0, 0] or K[0, 1] + SMALL < x_start: - logger.warning("The initial velocity is not controllable. {:f} not in ({:f}, {:f})".format( - x_start, K[0, 0], K[0, 1] - )) + logger.warning( + "The initial velocity is not controllable. {:f} not in ({:f}, {:f})".format( + x_start, K[0, 0], K[0, 1] + ) + ) if return_data: return None, None, None, K else: @@ -312,17 +350,24 @@ def compute_parameterization(self, sd_start, sd_end, return_data=False): # next line: simply reduce the current velocity by 0.1% or # a very small value (TINY) and choose the large value. if tries < MAX_TRIES: - xs[i] = max(xs[i] - TINY, 0.999 * xs[i]) # a slightly more aggressive reduction + xs[i] = max( + xs[i] - TINY, 0.999 * xs[i] + ) # a slightly more aggressive reduction tries += 1 logger.warning( "A numerical error occurs: the instance is controllable " "but forward pass fails. Attempt to try again with x[i] " "slightly reduced.\n" - "x[{:d}] reduced from {:.6f} to {:.6f}".format(i, xs[i] + SMALL, xs[i])) + "x[{:d}] reduced from {:.6f} to {:.6f}".format( + i, xs[i] + SMALL, xs[i] + ) + ) else: - logger.warning("Number of trials (to reduce xs[i]) reaches limits. " - "Compute parametrization fails!") - xs[i + 1:] = np.nan + logger.warning( + "Number of trials (to reduce xs[i]) reaches limits. " + "Compute parametrization fails!" + ) + xs[i + 1 :] = np.nan break else: tries = 0 @@ -334,9 +379,12 @@ def compute_parameterization(self, sd_start, sd_end, return_data=False): # this condition. x_next = xs[i] + 2 * deltas[i] * us[i] x_next = max(x_next - TINY, 0.9999 * x_next) - xs[i + 1] = min(K[i + 1, 1], - max(K[i + 1, 0], x_next)) - logger.debug("[Forward pass] u[{:d}] = {:f}, x[{:d}] = {:f}".format(i, us[i], i + 1, xs[i + 1])) + xs[i + 1] = min(K[i + 1, 1], max(K[i + 1, 0], x_next)) + logger.debug( + "[Forward pass] u[{:d}] = {:f}, x[{:d}] = {:f}".format( + i, us[i], i + 1, xs[i + 1] + ) + ) v_vec[i] = optim_res[2:] i += 1 self.solver_wrapper.close_solver() @@ -355,19 +403,23 @@ def _one_step_forward(self, i, L_current, feasible_set_next): return res nV = self.solver_wrapper.get_no_vars() g_upper = np.zeros(nV) - deltas = self.solver_wrapper.get_deltas()[i-1] - g_upper[0] = - 2 * deltas - g_upper[1] = - 1 + deltas = self.solver_wrapper.get_deltas()[i - 1] + g_upper[0] = -2 * deltas + g_upper[1] = -1 x_next_min = feasible_set_next[0] x_next_max = feasible_set_next[1] - opt_1 = self.solver_wrapper.solve_stagewise_optim(i, None, g_upper, L_current[0], L_current[1], x_next_min, x_next_max) + opt_1 = self.solver_wrapper.solve_stagewise_optim( + i, None, g_upper, L_current[0], L_current[1], x_next_min, x_next_max + ) x_opt_1 = opt_1[1] u_opt_1 = opt_1[0] x_upper = x_opt_1 + 2 * deltas * u_opt_1 - opt_0 = self.solver_wrapper.solve_stagewise_optim(i, None, - g_upper, L_current[0], L_current[1], x_next_min, x_next_max) + opt_0 = self.solver_wrapper.solve_stagewise_optim( + i, None, -g_upper, L_current[0], L_current[1], x_next_min, x_next_max + ) x_opt_0 = opt_0[1] u_opt_0 = opt_0[0] x_lower = x_opt_0 + 2 * deltas * u_opt_0 @@ -387,10 +439,14 @@ def compute_reachable_sets(self, sdmin, sdmax): if L[i + 1, 0] < 0: L[i + 1, 0] = 0 if np.isnan(L[i + 1]).any(): - logger.warn("L[{:d}]={:}. Path not parametrizable.".format(i + 1, L[i + 1])) + logger.warn( + "L[{:d}]={:}. Path not parametrizable.".format(i + 1, L[i + 1]) + ) return L if logger.isEnabledFor(logging.DEBUG): - logger.debug("[Compute reachable sets] L_{:d}={:}".format(i + 1, L[i + 1])) + logger.debug( + "[Compute reachable sets] L_{:d}={:}".format(i + 1, L[i + 1]) + ) self.solver_wrapper.close_solver() return L diff --git a/toppra/algorithm/reachabilitybased/time_optimal_algorithm.py b/toppra/algorithm/reachabilitybased/time_optimal_algorithm.py index a8647a56..58433e3a 100644 --- a/toppra/algorithm/reachabilitybased/time_optimal_algorithm.py +++ b/toppra/algorithm/reachabilitybased/time_optimal_algorithm.py @@ -9,28 +9,45 @@ class TOPPRA(ReachabilityAlgorithm): """Time-Optimal Path Parameterization based on Reachability Analysis (TOPPRA). + Examples + ----------- + >>> instance = algo.TOPPRA([pc_vel, pc_acc], path) + >>> jnt_traj = instance.compute_trajectory() # rest-to-rest motion + >>> instance.problem_data # intermediate result Parameters ---------- - constraint_list: :class:`~toppra.constraint.Constraint` [] + constraint_list: List[:class:`~toppra.constraint.Constraint`] List of constraints to which the robotic system is subjected to. - path: :class:`~toppra.Interpolator` + path: :class:`.AbstractGeometricPath` Input geometric path. - gridpoints: array, optional + gridpoints: Optional[np.ndarray] Gridpoints for discretization of the geometric path. The start - and end points must agree with the geometric path's domain. + and end points must agree with the geometric path's + `path_interval`. If omited a gridpoint will be automatically + selected. solver_wrapper: str, optional - Name of the solver wrapper to use. - See :class:`toppra.solverwrapper.hotqpOASESSolverWrapper`, - :class:`toppra.solverwrapper.seidelWrapper` + Name of the solver wrapper to use. Possible value are: + + - 'seidel' + - 'hotqpoases' + + For more details see the solverwrappers documentation. + Notes ----- In addition to the given constraints, there are additional constraints on the solutions enforced by the solver-warpper. - Therefore, different parametrizations are returned for different solver - wrappers. However, the different should be very small, especially - for well-conditioned problems. + Therefore, different parametrizations are returned for different + solver wrappers. However, the difference should be very small, + especially for well-conditioned problems. + + See also + -------- + :class:`toppra.solverwrapper.seidelWrapper` + :class:`toppra.solverwrapper.hotqpOASESSolverWrapper` + """ def _forward_step(self, i, x, K_next): @@ -60,14 +77,14 @@ def _forward_step(self, i, x, K_next): nV = self.solver_wrapper.get_no_vars() g_upper = np.zeros(nV) - g_upper[1] = - 1 - g_upper[0] = - 2 * self.solver_wrapper.get_deltas()[i] + g_upper[1] = -1 + g_upper[0] = -2 * self.solver_wrapper.get_deltas()[i] # Account for propagating numerical errors K_next_max = K_next[1] K_next_min = K_next[0] optim_var = self.solver_wrapper.solve_stagewise_optim( - i, None, g_upper, x, x, K_next_min, K_next_max) + i, None, g_upper, x, x, K_next_min, K_next_max + ) return optim_var - diff --git a/toppra/constants.py b/toppra/constants.py index 67d08617..fd16438a 100644 --- a/toppra/constants.py +++ b/toppra/constants.py @@ -1,11 +1,13 @@ """Some constants used by TOPPRA solvers.""" import logging + try: import openravepy as orpy + FOUND_OPENRAVE = True except (ImportError, SyntaxError) as err: FOUND_OPENRAVE = False - logging.getLogger('toppra').debug("Unable to import openrave.") + logging.getLogger("toppra").debug("Unable to import openrave.") # Constants diff --git a/toppra/constraint/__init__.py b/toppra/constraint/__init__.py index 106bf6f0..335fc4ca 100644 --- a/toppra/constraint/__init__.py +++ b/toppra/constraint/__init__.py @@ -1,8 +1,68 @@ -"""This module defines different dynamic constraints.""" +"""Modules implementing different dynamics constraints. + +Base abstractions and enum +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. autoclass:: toppra.constraint.Constraint + :members: compute_constraint_params, set_discretization_type, __repr__ + +.. autoclass:: toppra.constraint.ConstraintType + :members: + +.. autoclass:: toppra.constraint.LinearConstraint + :members: + :special-members: + :show-inheritance: + +.. autoclass:: toppra.constraint.DiscretizationType + :members: + +Velocity Constraints +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. autoclass:: toppra.constraint.JointVelocityConstraint + :members: + :special-members: + :show-inheritance: + +.. autoclass:: toppra.constraint.JointVelocityConstraintVarying + :members: + :special-members: + :show-inheritance: + +Second Order Constraints +^^^^^^^^^^^^^^^^^^^^^^^^^ +.. autoclass:: toppra.constraint.SecondOrderConstraint + :members: + :special-members: + :show-inheritance: + +.. autoclass:: toppra.constraint.JointTorqueConstraint + :members: + :special-members: + :show-inheritance: + +.. autoclass:: toppra.constraint.JointAccelerationConstraint + :members: + :special-members: + :show-inheritance: + +Robust Linear Constraint +^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. autoclass:: toppra.constraint.RobustLinearConstraint + :members: + :show-inheritance: + +Misc +^^^^^^^^^^ +.. autofunction:: toppra.constraint.canlinear_colloc_to_interpolate + +""" from .constraint import ConstraintType, DiscretizationType, Constraint from .joint_torque import JointTorqueConstraint from .linear_joint_acceleration import JointAccelerationConstraint -from .linear_joint_velocity import JointVelocityConstraint, JointVelocityConstraintVarying +from .linear_joint_velocity import ( + JointVelocityConstraint, + JointVelocityConstraintVarying, +) from .linear_second_order import SecondOrderConstraint, canlinear_colloc_to_interpolate from .conic_constraint import RobustLinearConstraint from .linear_constraint import LinearConstraint diff --git a/toppra/constraint/conic_constraint.py b/toppra/constraint/conic_constraint.py index 9d53ad46..712ae8eb 100644 --- a/toppra/constraint/conic_constraint.py +++ b/toppra/constraint/conic_constraint.py @@ -32,6 +32,7 @@ class ConicConstraint(Constraint): (Vol. 2). Siam. """ + def __init__(self): self.constraint_type = ConstraintType.CanonicalConic self.discretization_type = DiscretizationType.Collocation @@ -68,21 +69,34 @@ class RobustLinearConstraint(ConicConstraint): Constraint discretization scheme to use. """ - def __init__(self, cnst, ellipsoid_axes_lengths, discretization_scheme=DiscretizationType.Collocation): + + def __init__( + self, + cnst, + ellipsoid_axes_lengths, + discretization_scheme=DiscretizationType.Collocation, + ): super(RobustLinearConstraint, self).__init__() self.dof = cnst.get_dof() assert cnst.get_constraint_type() == ConstraintType.CanonicalLinear self.set_discretization_type(discretization_scheme) if np.any(np.r_[ellipsoid_axes_lengths] < 0): - raise ValueError("Perturbation must be non-negative. Input {:}".format(ellipsoid_axes_lengths)) + raise ValueError( + "Perturbation must be non-negative. Input {:}".format( + ellipsoid_axes_lengths + ) + ) self.base_constraint = cnst self.ellipsoid_axes_lengths = ellipsoid_axes_lengths - self._format_string += " Robust constraint generated from a canonical linear constraint\n" + self._format_string += ( + " Robust constraint generated from a canonical linear constraint\n" + ) def compute_constraint_params(self, path, gridpoints, scaling): self.base_constraint.set_discretization_type(self.discretization_type) a_, b_, c_, F_, g_, u_, x_ = self.base_constraint.compute_constraint_params( - path, gridpoints, scaling) + path, gridpoints, scaling + ) N = len(gridpoints) - 1 if self.base_constraint.identical: d = F_.shape[0] # number of rows diff --git a/toppra/constraint/constraint.py b/toppra/constraint/constraint.py index 94f170ae..0ba63830 100644 --- a/toppra/constraint/constraint.py +++ b/toppra/constraint/constraint.py @@ -1,26 +1,33 @@ """Base class for all path parametrization contraints. """ from enum import Enum import logging +import numpy as np +from ..interpolator import AbstractGeometricPath + logger = logging.getLogger(__name__) class ConstraintType(Enum): """Type of path parametrization constraint.""" + + #: Unknown Unknown = -1 + #: Simple linear constraints with only linear inequalities CanonicalLinear = 0 + #: Linear constraints with linear conic inequalities. CanonicalConic = 1 class DiscretizationType(Enum): """Enum to mark different Discretization Scheme for constraint. - 1. `Collocation`: smaller problem size, but lower accuracy. - 2. `Interplation`: larger problem size, but higher accuracy. - In general, the difference in speed is not too large. Should use Interpolation if possible. """ + #: Smaller problem size, but lower accuracy. Collocation = 0 + + #: Larger problem size, but higher accuracy. Interpolation = 1 @@ -28,11 +35,13 @@ class Constraint(object): """The base constraint class.""" def __repr__(self): - string = self.__class__.__name__ + '(\n' - string += ' Type: {:}'.format(self.constraint_type) + '\n' - string += ' Discretization Scheme: {:}'.format(self.discretization_type) + '\n' + string = self.__class__.__name__ + "(\n" + string += " Type: {:}".format(self.constraint_type) + "\n" + string += ( + " Discretization Scheme: {:}".format(self.discretization_type) + "\n" + ) string += self._format_string - string += ')' + string += ")" return string def get_dof(self): @@ -77,11 +86,18 @@ def set_discretization_type(self, discretization_type): self.discretization_type = DiscretizationType.Collocation elif discretization_type == 1: self.discretization_type = DiscretizationType.Interpolation - elif discretization_type == DiscretizationType.Collocation or discretization_type == DiscretizationType.Interpolation: + elif ( + discretization_type == DiscretizationType.Collocation + or discretization_type == DiscretizationType.Interpolation + ): self.discretization_type = discretization_type else: - raise "Discretization type: {:} not implemented!".format(discretization_type) + raise "Discretization type: {:} not implemented!".format( + discretization_type + ) - def compute_constraint_params(self, *args, **kwargs): + def compute_constraint_params( + self, path: AbstractGeometricPath, gridpoints: np.ndarray, *args, **kwargs + ): """Evaluate parameters of the constraint.""" raise NotImplementedError diff --git a/toppra/constraint/joint_torque.py b/toppra/constraint/joint_torque.py index b1b761b0..7ad6e680 100644 --- a/toppra/constraint/joint_torque.py +++ b/toppra/constraint/joint_torque.py @@ -52,7 +52,14 @@ class JointTorqueConstraint(LinearConstraint): higher computational cost. """ - def __init__(self, inv_dyn, tau_lim, fs_coef, discretization_scheme=DiscretizationType.Collocation): + + def __init__( + self, + inv_dyn, + tau_lim, + fs_coef, + discretization_scheme=DiscretizationType.Collocation, + ): super(JointTorqueConstraint, self).__init__() self.inv_dyn = inv_dyn self.tau_lim = np.array(tau_lim, dtype=float) @@ -62,14 +69,18 @@ def __init__(self, inv_dyn, tau_lim, fs_coef, discretization_scheme=Discretizati assert self.tau_lim.shape[1] == 2, "Wrong input shape." self._format_string = " Torque limit: \n" for i in range(self.tau_lim.shape[0]): - self._format_string += " J{:d}: {:}".format(i + 1, self.tau_lim[i]) + "\n" + self._format_string += ( + " J{:d}: {:}".format(i + 1, self.tau_lim[i]) + "\n" + ) self.identical = True def compute_constraint_params(self, path, gridpoints, scaling): if path.dof != self.get_dof(): - raise ValueError("Wrong dimension: constraint dof ({:d}) not equal to path dof ({:d})".format( - self.get_dof(), path.dof - )) + raise ValueError( + "Wrong dimension: constraint dof ({:d}) not equal to path dof ({:d})".format( + self.get_dof(), path.dof + ) + ) v_zero = np.zeros(path.dof) p = path.eval(gridpoints / scaling) ps = path.evald(gridpoints / scaling) / scaling @@ -80,29 +91,26 @@ def compute_constraint_params(self, path, gridpoints, scaling): F = np.zeros((dof * 2, dof)) g = np.zeros(dof * 2) g[0:dof] = self.tau_lim[:, 1] - g[dof:] = - self.tau_lim[:, 0] + g[dof:] = -self.tau_lim[:, 0] F[0:dof, :] = I_dof F[dof:, :] = -I_dof - c = np.array( - [self.inv_dyn(p_, v_zero, v_zero) for p_ in p] + c = np.array([self.inv_dyn(p_, v_zero, v_zero) for p_ in p]) + a = np.array([self.inv_dyn(p_, v_zero, ps_) for p_, ps_ in zip(p, ps)]) - c + b = ( + np.array([self.inv_dyn(p_, ps_, pss_) for p_, ps_, pss_ in zip(p, ps, pss)]) + - c ) - a = np.array( - [self.inv_dyn(p_, v_zero, ps_) for p_, ps_ in zip(p, ps)] - ) - c - b = np.array( - [self.inv_dyn(p_, ps_, pss_) for p_, ps_, pss_ in zip(p, ps, pss)] - ) - c # dry friction for i in range(0, dof): - c[:,i] += self.fs_coef[i] * np.sign(ps[:,i]) + c[:, i] += self.fs_coef[i] * np.sign(ps[:, i]) if self.discretization_type == DiscretizationType.Collocation: return a, b, c, F, g, None, None elif self.discretization_type == DiscretizationType.Interpolation: - return canlinear_colloc_to_interpolate(a, b, c, F, g, None, None, - gridpoints, identical=True) + return canlinear_colloc_to_interpolate( + a, b, c, F, g, None, None, gridpoints, identical=True + ) else: raise NotImplementedError("Other form of discretization not supported!") - diff --git a/toppra/constraint/linear_constraint.py b/toppra/constraint/linear_constraint.py index 27e36df0..0ebfd515 100644 --- a/toppra/constraint/linear_constraint.py +++ b/toppra/constraint/linear_constraint.py @@ -1,7 +1,7 @@ """This module defines the abstract linear constraint class.""" import numpy as np -from .constraint import Constraint -from .constraint import ConstraintType, DiscretizationType +from .constraint import ConstraintType, DiscretizationType, Constraint +from ..interpolator import AbstractGeometricPath class LinearConstraint(Constraint): @@ -48,7 +48,9 @@ def __init__(self): self.n_extra_vars = 0 self.identical = False - def compute_constraint_params(self, path, gridpoints, scaling=1): + def compute_constraint_params( + self, path: AbstractGeometricPath, gridpoints: np.ndarray, *args, **kwargs + ): """Compute numerical coefficients of the given constraint. Parameters diff --git a/toppra/constraint/linear_joint_acceleration.py b/toppra/constraint/linear_joint_acceleration.py index f8a50995..8eabcde7 100644 --- a/toppra/constraint/linear_joint_acceleration.py +++ b/toppra/constraint/linear_joint_acceleration.py @@ -2,6 +2,7 @@ import numpy as np from .linear_constraint import LinearConstraint, canlinear_colloc_to_interpolate from ..constraint import DiscretizationType +from ..interpolator import AbstractGeometricPath class JointAccelerationConstraint(LinearConstraint): @@ -59,27 +60,45 @@ def __init__(self, alim, discretization_scheme=DiscretizationType.Interpolation) self._format_string += " J{:d}: {:}".format(i + 1, self.alim[i]) + "\n" self.identical = True - def compute_constraint_params(self, path, gridpoints, scaling=None): - # type: (Path, np.ndarray, Optional[float]) -> Any + def compute_constraint_params( + self, path: AbstractGeometricPath, gridpoints: np.ndarray, *args, **kwargs + ): if path.dof != self.dof: - raise ValueError("Wrong dimension: constraint dof ({:d}) not equal to path dof ({:d})".format( - self.dof, path.dof - )) - if scaling is None: - scaling = 1 - ps_vec = (path(gridpoints / scaling, order=1) / scaling).reshape((-1, path.dof)) - pss_vec = (path(gridpoints / scaling, order=2) / scaling ** 2).reshape((-1, path.dof)) + raise ValueError( + "Wrong dimension: constraint dof ({:d}) not equal to path dof ({:d})".format( + self.dof, path.dof + ) + ) + ps_vec = (path(gridpoints, order=1)).reshape((-1, path.dof)) + pss_vec = (path(gridpoints, order=2)).reshape((-1, path.dof)) dof = path.dof F_single = np.zeros((dof * 2, dof)) g_single = np.zeros(dof * 2) g_single[0:dof] = self.alim[:, 1] - g_single[dof:] = - self.alim[:, 0] + g_single[dof:] = -self.alim[:, 0] F_single[0:dof, :] = np.eye(dof) F_single[dof:, :] = -np.eye(dof) if self.discretization_type == DiscretizationType.Collocation: - return ps_vec, pss_vec, np.zeros_like(ps_vec), F_single, g_single, None, None + return ( + ps_vec, + pss_vec, + np.zeros_like(ps_vec), + F_single, + g_single, + None, + None, + ) elif self.discretization_type == DiscretizationType.Interpolation: - return canlinear_colloc_to_interpolate(ps_vec, pss_vec, np.zeros_like(ps_vec), F_single, g_single, None, None, - gridpoints, identical=True) + return canlinear_colloc_to_interpolate( + ps_vec, + pss_vec, + np.zeros_like(ps_vec), + F_single, + g_single, + None, + None, + gridpoints, + identical=True, + ) else: raise NotImplementedError("Other form of discretization not supported!") diff --git a/toppra/constraint/linear_joint_velocity.py b/toppra/constraint/linear_joint_velocity.py index e23f045b..79accd9b 100644 --- a/toppra/constraint/linear_joint_velocity.py +++ b/toppra/constraint/linear_joint_velocity.py @@ -12,7 +12,7 @@ class JointVelocityConstraint(LinearConstraint): ---------- vlim: np.ndarray Shape (dof, 2). The lower and upper velocity bounds of the j-th joint - are given by alim[j, 0] and alim[j, 1] respectively. + are given by vlim[j, 0] and vlim[j, 1] respectively. """ @@ -78,7 +78,7 @@ def compute_constraint_params(self, path, gridpoints, scaling=1): raise ValueError( "Wrong dimension: constraint dof ({:d}) not equal to path dof ({:d})" .format(self.get_dof(), path.dof)) - qs = path.evald(gridpoints / scaling) / scaling + qs = path((gridpoints / scaling), 1) / scaling vlim_grid = np.array([self.vlim_func(s) for s in gridpoints]) _, _, xbound_ = _create_velocity_constraint_varying(qs, vlim_grid) xbound = np.array(xbound_) diff --git a/toppra/exceptions.py b/toppra/exceptions.py index 7f35aba7..41767345 100644 --- a/toppra/exceptions.py +++ b/toppra/exceptions.py @@ -3,3 +3,11 @@ class ToppraError(Exception): """A generic error class used in the toppra library.""" + + +class BadInputVelocities(ToppraError): + """Raise when given input velocity is invalid.""" + + +class SolverNotFound(ToppraError): + """Unable to find a solver.""" diff --git a/toppra/interpolator.py b/toppra/interpolator.py index a0eb05c4..3a818f8b 100644 --- a/toppra/interpolator.py +++ b/toppra/interpolator.py @@ -1,5 +1,12 @@ -"""This module implements interpolators for representing geometric paths and trajectories. +"""Algorithms implemented in :mod:`toppra` requires geometric paths +that implements the abstract class + +:class:`toppra.interpolator.AbstractGeometricPath`. The interface is +really simple, requiring only the evaluted values, as well as the +first and second derivative. + """ +from typing import List import logging import numpy as np from scipy.interpolate import UnivariateSpline, CubicSpline, PPoly @@ -12,27 +19,45 @@ import openravepy as orpy # pylint: disable=import-error -def propose_gridpoints(path, max_err_threshold=1e-4, max_iteration=100, max_seg_length=0.05): - """Generate a set of grid pooint for the given path. +def propose_gridpoints( + path, max_err_threshold=1e-4, max_iteration=100, max_seg_length=0.05 +): + r"""Generate gridpoints that sufficiently cover the given path. This function operates in multiple passes through the geometric path from the start to the end point. In each pass, for each segment, the maximum interpolation error is estimated using the following equation: - 0.5 * max(abs(p'' * d_segment ^ 2)) + .. math:: + + err_{est} = 0.5 * \mathrm{max}(\mathrm{abs}(p'' * d_{segment} ^ 2)) + + Here :math:`p''` is the second derivative of the path and + d_segment is the length of the segment. If the estimated error + :math:`err_{test}` is greater than the given threshold + `max_err_threshold` then the segment is divided in two half. - Here p'' is the second derivative of the path and d_segment is the - length of the segment. Intuitively, at positions with higher - curvature, there must be more points in order to improve - approximation quality. + Intuitively, at positions with higher curvature, there must be + more points in order to improve approximation + quality. Theoretically toppra performs the best when the proposed + gridpoint is optimally distributed. Arguments --------- - path: Input geometric path. - max_err_threshold: Maximum worstcase error thrshold allowable. - max_iteration: Maximum number of iterations. - max_seg_length: All segments length should be smaller than this value. + path: :class:`AbstractGeometricPath` + Input geometric path. + max_err_threshold: float + Maximum worstcase error thrshold allowable. + max_iteration: int + Maximum number of iterations. + max_seg_length: float + All segments length should be smaller than this value. + + Returns + ---------- + gridpoints_ept: np.ndarray(N,) + The proposed gridpoints. """ gridpoints_ept = [path.path_interval[0], path.path_interval[1]] @@ -58,10 +83,10 @@ def propose_gridpoints(path, max_err_threshold=1e-4, max_iteration=100, max_seg_ return gridpoints_ept -class AbstractGeometricPath(object): - """Base geometric path. +class AbstractGeometricPath: + """The base class to represent geometric paths. - Derive geometric paths classes should derive the below abstract methods. + Derive geometric paths classes should implement the below abstract methods. """ def __call__(self, path_positions, order=0): @@ -81,14 +106,17 @@ def __call__(self, path_positions, order=0): Returns ------- - np.ndarray - Evaluated values. + np.ndarray(N, dof) + The evaluated joint positions, velocity or + accelerations. The shape of the result depends on the + shape of the input. """ raise NotImplementedError @property def dof(self): + # type: () -> int """Return the degrees-of-freedom of the path.""" raise NotImplementedError @@ -98,12 +126,17 @@ def path_interval(self): Returns ------- - out: + np.ndarray(2,) The starting and ending path positions. """ raise NotImplementedError + @property + def waypoints(self): + """Tuple[ndarray, ndarray] or None: The path's waypoints if applicable. None otherwise.""" + return None + class RaveTrajectoryWrapper(AbstractGeometricPath): """An interpolator that wraps OpenRAVE's :class:`GenericTrajectory`. @@ -115,10 +148,11 @@ class RaveTrajectoryWrapper(AbstractGeometricPath): object. """ + @staticmethod def _extract_interpolation_method(spec): - _interpolation = spec.GetGroupFromName('joint').interpolation - if _interpolation not in ['quadratic', 'cubic']: + _interpolation = spec.GetGroupFromName("joint").interpolation + if _interpolation not in ["quadratic", "cubic"]: raise ValueError( "This class only handles trajectories with quadratic or cubic interpolation" ) @@ -137,29 +171,34 @@ def __init__(self, traj, robot): """ super(RaveTrajectoryWrapper, self).__init__() - self.traj = traj #: init + self.traj = traj spec = traj.GetConfigurationSpecification() self._dof = robot.GetActiveDOF() self._interpolation = self._extract_interpolation_method(spec) self._duration = traj.GetDuration() all_waypoints = traj.GetWaypoints(0, traj.GetNumWaypoints()).reshape( - traj.GetNumWaypoints(), -1) + traj.GetNumWaypoints(), -1 + ) valid_wp_indices = [0] - self.ss_waypoints = [0.0] + ss_waypoints: List[float] = [0.0] for i in range(1, traj.GetNumWaypoints()): dt = spec.ExtractDeltaTime(all_waypoints[i]) if dt > 1e-5: # If delta is too small, skip it. valid_wp_indices.append(i) - self.ss_waypoints.append(self.ss_waypoints[-1] + dt) - self.ss_waypoints = np.array(self.ss_waypoints) + ss_waypoints.append(ss_waypoints[-1] + dt) + self.ss_waypoints = np.array(ss_waypoints) n_waypoints = len(valid_wp_indices) def _extract_waypoints(order): - return np.array([ - spec.ExtractJointValues(all_waypoints[i], robot, robot.GetActiveDOFIndices(), order) - for i in valid_wp_indices - ]) + return np.array( + [ + spec.ExtractJointValues( + all_waypoints[i], robot, robot.GetActiveDOFIndices(), order + ) + for i in valid_wp_indices + ] + ) def _make_ppoly(): if n_waypoints == 1: @@ -174,8 +213,9 @@ def _make_ppoly(): waypoints_d = _extract_waypoints(1) waypoints_dd = [] for i in range(n_waypoints - 1): - qdd = ((waypoints_d[i + 1] - waypoints_d[i]) / - (self.ss_waypoints[i + 1] - self.ss_waypoints[i])) + qdd = (waypoints_d[i + 1] - waypoints_d[i]) / ( + self.ss_waypoints[i + 1] - self.ss_waypoints[i] + ) waypoints_dd.append(qdd) waypoints_dd = np.array(waypoints_dd) @@ -186,7 +226,7 @@ def _make_ppoly(): pp_coeffs[:, iseg, idof] = [ waypoints_dd[iseg, idof] / 2, waypoints_d[iseg, idof], - waypoints[iseg, idof] + waypoints[iseg, idof], ] return PPoly(pp_coeffs, self.ss_waypoints) @@ -196,8 +236,9 @@ def _make_ppoly(): waypoints_dd = _extract_waypoints(2) waypoints_ddd = [] for i in range(n_waypoints - 1): - qddd = ((waypoints_dd[i + 1] - waypoints_dd[i]) / - (self.ss_waypoints[i + 1] - self.ss_waypoints[i])) + qddd = (waypoints_dd[i + 1] - waypoints_dd[i]) / ( + self.ss_waypoints[i + 1] - self.ss_waypoints[i] + ) waypoints_ddd.append(qddd) waypoints_ddd = np.array(waypoints_ddd) @@ -209,7 +250,7 @@ def _make_ppoly(): waypoints_ddd[iseg, idof] / 6, waypoints_dd[iseg, idof] / 2, waypoints_d[iseg, idof], - waypoints[iseg, idof] + waypoints[iseg, idof], ] return PPoly(pp_coeffs, self.ss_waypoints) raise ValueError("An error has occured. Unable to form PPoly.") @@ -270,43 +311,37 @@ class SplineInterpolator(AbstractGeometricPath): Parameters ---------- - ss_waypoints: array - Shaped (N+1,). Path positions of the waypoints. - waypoints: array - Shaped (N+1, dof). Waypoints. + ss_waypoints: np.ndarray(m,) + Path positions of the waypoints. + waypoints: np.ndarray(m, d) + Waypoints. bc_type: optional Boundary conditions of the spline. Can be 'not-a-knot', 'clamped', 'natural' or 'periodic'. - - 'not-a-knot': The most default option, return to natural + - 'not-a-knot': The most default option, return the most naturally looking spline. - 'clamped': First-order derivatives of the spline at the two - end are zeroed. + end are clamped at zero. See scipy.CubicSpline documentation for more details. - Attributes - ---------- - dof : int - Output dimension of the function - cspl : :class:`scipy.interpolate.CubicSpline` - The underlying cubic spline. """ - def __init__(self, ss_waypoints, waypoints, bc_type='not-a-knot'): + def __init__(self, ss_waypoints, waypoints, bc_type="not-a-knot"): super(SplineInterpolator, self).__init__() self.ss_waypoints = np.array(ss_waypoints) # type: np.ndarray - self.waypoints = np.array(waypoints) # type: np.ndarray - assert self.ss_waypoints.shape[0] == self.waypoints.shape[0] + self._q_waypoints = np.array(waypoints) # type: np.ndarray + assert self.ss_waypoints.shape[0] == self._q_waypoints.shape[0] if len(ss_waypoints) == 1: def _1dof_cspl(s): try: ret = np.zeros((len(s), self.dof)) - ret[:, :] = self.waypoints[0] + ret[:, :] = self._q_waypoints[0] except TypeError: - ret = self.waypoints[0] + ret = self._q_waypoints[0] return ret def _1dof_cspld(s): @@ -333,9 +368,15 @@ def __call__(self, path_positions, order=0): return self.cspldd(path_positions) raise ValueError("Invalid order %s" % order) - def get_waypoints(self): - """Return the appropriate scaled waypoints.""" - return self.ss_waypoints, self.waypoints + @property + def waypoints(self): + """Tuple[np.ndarray, np.ndarray]: Return the waypoints. + + The first element is the positions, the second element is the + array of waypoints. + + """ + return self.ss_waypoints, self._q_waypoints @deprecated def get_duration(self): @@ -359,9 +400,9 @@ def get_path_interval(self): @property def dof(self): - if np.isscalar(self.waypoints[0]): + if np.isscalar(self._q_waypoints[0]): return 1 - return self.waypoints[0].shape[0] + return self._q_waypoints[0].shape[0] @deprecated def get_dof(self): # type: () -> int @@ -398,7 +439,7 @@ def compute_rave_trajectory(self, robot): """ traj = orpy.RaveCreateTrajectory(robot.GetEnv(), "") - spec = robot.GetActiveConfigurationSpecification('cubic') + spec = robot.GetActiveConfigurationSpecification("cubic") spec.AddDerivativeGroups(1, False) spec.AddDerivativeGroups(2, True) @@ -410,15 +451,16 @@ def compute_rave_trajectory(self, robot): q = self(0) qd = self(0, 1) qdd = self(0, 2) - traj.Insert(traj.GetNumWaypoints(), - list(q) + list(qd) + list(qdd) + [0]) + traj.Insert(traj.GetNumWaypoints(), list(q) + list(qd) + list(qdd) + [0]) else: qs = self(self.ss_waypoints) qds = self(self.ss_waypoints, 1) qdds = self(self.ss_waypoints, 2) for (q, qd, qdd, dt) in zip(qs, qds, qdds, deltas): - traj.Insert(traj.GetNumWaypoints(), - q.tolist() + qd.tolist() + qdd.tolist() + [dt]) + traj.Insert( + traj.GetNumWaypoints(), + q.tolist() + qd.tolist() + qdd.tolist() + [dt], + ) return traj @@ -440,16 +482,17 @@ def __init__(self, ss_waypoints, waypoints): super(UnivariateSplineInterpolator, self).__init__() assert ss_waypoints[0] == 0, "First index must equals zero." self.ss_waypoints = np.array(ss_waypoints) - self.waypoints = np.array(waypoints) + self._q_waypoints = np.array(waypoints) if np.isscalar(waypoints[0]): self._dof = 1 else: self._dof = waypoints[0].shape[0] - assert self.ss_waypoints.shape[0] == self.waypoints.shape[0] + assert self.ss_waypoints.shape[0] == self._q_waypoints.shape[0] self.uspl = [] for i in range(self.dof): self.uspl.append( - UnivariateSpline(self.ss_waypoints, self.waypoints[:, i])) + UnivariateSpline(self.ss_waypoints, self._q_waypoints[:, i]) + ) self.uspld = [spl.derivative() for spl in self.uspl] self.uspldd = [spl.derivative() for spl in self.uspld] @@ -535,8 +578,7 @@ def __init__(self, coeff, s_start=0.0, s_end=1.0): self.coeff = self.coeff.reshape(1, -1) else: self.poly = [ - np.polynomial.Polynomial(self.coeff[i]) - for i in range(self.dof) + np.polynomial.Polynomial(self.coeff[i]) for i in range(self.dof) ] self.polyd = [poly.deriv() for poly in self.poly] diff --git a/toppra/planning_utils.py b/toppra/planning_utils.py index c950297d..96bb425b 100644 --- a/toppra/planning_utils.py +++ b/toppra/planning_utils.py @@ -1,6 +1,10 @@ from .interpolator import RaveTrajectoryWrapper, SplineInterpolator -from .constraint import JointAccelerationConstraint, JointVelocityConstraint, \ - DiscretizationType, SecondOrderConstraint +from .constraint import ( + JointAccelerationConstraint, + JointVelocityConstraint, + DiscretizationType, + SecondOrderConstraint, +) from .algorithm import TOPPRA import numpy as np import logging @@ -9,15 +13,17 @@ logger = logging.getLogger(__name__) -def retime_active_joints_kinematics(traj, - robot, - output_interpolator=False, - vmult=1.0, - amult=1.0, - N=100, - use_ravewrapper=False, - additional_constraints=[], - solver_wrapper='hotqpoases'): +def retime_active_joints_kinematics( + traj, + robot, + output_interpolator=False, + vmult=1.0, + amult=1.0, + N=100, + use_ravewrapper=False, + additional_constraints=[], + solver_wrapper="hotqpoases", +): """ Retime a trajectory wrt velocity and acceleration constraints. Parameters @@ -55,10 +61,9 @@ def retime_active_joints_kinematics(traj, if isinstance(traj, np.ndarray): logger.debug("Received a list of waypoints.") ss_waypoints = np.linspace(0, 1, len(traj)) - path = SplineInterpolator(ss_waypoints, traj, bc_type='natural') + path = SplineInterpolator(ss_waypoints, traj, bc_type="natural") elif use_ravewrapper: - logger.warning( - "Use RaveTrajectoryWrapper. This might not work properly!") + logger.warning("Use RaveTrajectoryWrapper. This might not work properly!") path = RaveTrajectoryWrapper(traj, robot) elif isinstance(traj, SplineInterpolator): path = traj @@ -77,8 +82,8 @@ def retime_active_joints_kinematics(traj, else: ss_waypoints.append(ss_waypoints[-1] + dt) waypoints.append( - spec.ExtractJointValues(data, robot, - robot.GetActiveDOFIndices())) + spec.ExtractJointValues(data, robot, robot.GetActiveDOFIndices()) + ) path = SplineInterpolator(ss_waypoints, waypoints) vmax = robot.GetActiveDOFMaxVel() * vmult @@ -88,9 +93,9 @@ def retime_active_joints_kinematics(traj, pc_vel = JointVelocityConstraint(vlim) pc_acc = JointAccelerationConstraint( - alim, discretization_scheme=DiscretizationType.Interpolation) - logger.debug( - "Number of constraints %d", 2 + len(additional_constraints)) + alim, discretization_scheme=DiscretizationType.Interpolation + ) + logger.debug("Number of constraints %d", 2 + len(additional_constraints)) logger.debug(str(pc_vel)) logger.debug(str(pc_acc)) for _c in additional_constraints: @@ -100,21 +105,27 @@ def retime_active_joints_kinematics(traj, ds = path.ss_waypoints[-1] / N gridpoints = [path.ss_waypoints[0]] for i in range(len(path.ss_waypoints) - 1): - Ni = int( - np.ceil((path.ss_waypoints[i + 1] - path.ss_waypoints[i]) / ds)) - gridpoints.extend(path.ss_waypoints[i] + - np.linspace(0, 1, Ni + 1)[1:] * - (path.ss_waypoints[i + 1] - path.ss_waypoints[i])) - instance = TOPPRA([pc_vel, pc_acc] + additional_constraints, - path, - gridpoints=gridpoints, - solver_wrapper=solver_wrapper) + Ni = int(np.ceil((path.ss_waypoints[i + 1] - path.ss_waypoints[i]) / ds)) + gridpoints.extend( + path.ss_waypoints[i] + + np.linspace(0, 1, Ni + 1)[1:] + * (path.ss_waypoints[i + 1] - path.ss_waypoints[i]) + ) + instance = TOPPRA( + [pc_vel, pc_acc] + additional_constraints, + path, + gridpoints=gridpoints, + solver_wrapper=solver_wrapper, + ) _t1 = time.time() traj_ra, aux_traj = instance.compute_trajectory(0, 0) _t2 = time.time() - logger.debug("t_setup={:.5f}ms, t_solve={:.5f}ms, t_total={:.5f}ms".format( - (_t1 - _t0) * 1e3, (_t2 - _t1) * 1e3, (_t2 - _t0) * 1e3)) + logger.debug( + "t_setup={:.5f}ms, t_solve={:.5f}ms, t_total={:.5f}ms".format( + (_t1 - _t0) * 1e3, (_t2 - _t1) * 1e3, (_t2 - _t0) * 1e3 + ) + ) if traj_ra is None: logger.warning("Retime fails.") traj_rave = None @@ -129,7 +140,8 @@ def retime_active_joints_kinematics(traj, def create_rave_torque_path_constraint( - robot, discretization_scheme=DiscretizationType.Collocation): + robot, discretization_scheme=DiscretizationType.Collocation +): """Create torque bound for the given robot. The torque bound constraint for a manipulator whose links are @@ -165,8 +177,7 @@ def inv_dyn(q, qd, qdd, s=0): return res[active_dofs] tau_max = robot.GetDOFTorqueLimits()[robot.GetActiveDOFIndices()] - F = np.vstack( - (np.eye(robot.GetActiveDOF()), -np.eye(robot.GetActiveDOF()))) + F = np.vstack((np.eye(robot.GetActiveDOF()), -np.eye(robot.GetActiveDOF()))) g = np.hstack((tau_max, tau_max)) def cnst_F(q): @@ -175,9 +186,11 @@ def cnst_F(q): def cnst_g(q): return g - cnst = SecondOrderConstraint(inv_dyn, - cnst_F, - cnst_g, - dof=robot.GetActiveDOF(), - discretization_scheme=discretization_scheme) + cnst = SecondOrderConstraint( + inv_dyn, + cnst_F, + cnst_g, + dof=robot.GetActiveDOF(), + discretization_scheme=discretization_scheme, + ) return cnst diff --git a/toppra/simplepath.py b/toppra/simplepath.py new file mode 100644 index 00000000..f4c581a5 --- /dev/null +++ b/toppra/simplepath.py @@ -0,0 +1,72 @@ +from typing import List, Union, Optional +import numpy as np +from scipy.interpolate import BPoly +from .interpolator import AbstractGeometricPath + + +class SimplePath(AbstractGeometricPath): + """A standard class for representing continuous multi-dimentional function. + + Args: + x: "Time instances" of the waypoints. + y: Function values at waypoints. + yd: First-derivative values. + ydd: Second-derivative values. + """ + + def __init__( + self, + x: np.ndarray, + y: np.ndarray, + yd: np.ndarray = None, + ydd: np.ndarray = None, + ): + if len(y.shape) == 1: + y = y.reshape(-1, 1) + if yd is not None and len(yd.shape) == 1: + yd = yd.reshape(-1, 1) + self._x = x + self._y = y.astype(float) + self._yd = yd if yd is None else yd.astype(float) + self._ydd = ydd if ydd is None else ydd.astype(float) + self._polys = self._construct_polynomials() + + def __call__(self, xi, order=0): + """Evaluate the path at given position.""" + ret = [poly.derivative(order)(xi) for poly in self._polys] + return np.array(ret) + + @property + def dof(self): + return self._y.shape[1] + + @property + def path_interval(self): + return np.array([self._x[0], self._x[-1]], dtype=float) + + @property + def waypoints(self): + return self._y + + def _autofill_yd(self): + if self._yd is None: + _yd = np.zeros_like(self._y[:], dtype=float) + for i in range(1, len(_yd) - 1): + _yd[i] = (self._y[i + 1] - self._y[i - 1]) / ( + self._x[i + 1] - self._x[i - 1] + ) + else: + _yd = np.array(self._yd[:]) + return _yd + + def _construct_polynomials(self): + polys = [] + _yd = self._autofill_yd() + + for j in range(self._y.shape[1]): + + y_with_derivatives = np.vstack((self._y[:, j], _yd[:, j])).T + poly = BPoly.from_derivatives(self._x, y_with_derivatives) + polys.append(poly) + + return polys diff --git a/toppra/solverwrapper/cvxpy_solverwrapper.py b/toppra/solverwrapper/cvxpy_solverwrapper.py index da32ac3c..2ad57f51 100644 --- a/toppra/solverwrapper/cvxpy_solverwrapper.py +++ b/toppra/solverwrapper/cvxpy_solverwrapper.py @@ -8,12 +8,14 @@ logger = logging.getLogger(__name__) try: import cvxpy + FOUND_CVXPY = True except ImportError: logger.info("CVXPY installation not found.") FOUND_CVXPY = False try: import mosek + FOUND_MOSEK = True except ImportError: logger.info("Mosek installation not found!") @@ -55,10 +57,7 @@ def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): ux = cvxpy.Variable(2) u = ux[0] x = ux[1] - cvxpy_constraints = [ - - CVXPY_MAXU <= u, u <= CVXPY_MAXU, - 0 <= x, x <= CVXPY_MAXX - ] + cvxpy_constraints = [-CVXPY_MAXU <= u, u <= CVXPY_MAXU, 0 <= x, x <= CVXPY_MAXX] if not np.isnan(x_min): cvxpy_constraints.append(x_min <= x) @@ -88,7 +87,7 @@ def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): # large bound. The below max(), min() operators is a # workaround to get pass this issue. if ubound is not None: - cvxpy_constraints.append(max(- CVXPY_MAXU, ubound[i, 0]) <= u) + cvxpy_constraints.append(max(-CVXPY_MAXU, ubound[i, 0]) <= u) cvxpy_constraints.append(u <= min(CVXPY_MAXU, ubound[i, 1])) if xbound is not None: @@ -102,11 +101,15 @@ def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): d = a.shape[1] for j in range(d): cvxpy_constraints.append( - a[i, j] * u + b[i, j] * x + c[i, j] - + cvxpy.norm(P[i, j].T[:, :2] * ux + P[i, j].T[:, 2]) <= 0) + a[i, j] * u + + b[i, j] * x + + c[i, j] + + cvxpy.norm(P[i, j].T[:, :2] * ux + P[i, j].T[:, 2]) + <= 0 + ) if ubound is not None: - cvxpy_constraints.append(max(- CVXPY_MAXU, ubound[i, 0]) <= u) + cvxpy_constraints.append(max(-CVXPY_MAXU, ubound[i, 0]) <= u) cvxpy_constraints.append(u <= min(CVXPY_MAXU, ubound[i, 1])) if xbound is not None: @@ -123,10 +126,12 @@ def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): except cvxpy.SolverError: # solve fail pass - if problem.status == cvxpy.OPTIMAL or problem.status == cvxpy.OPTIMAL_INACCURATE: + if ( + problem.status == cvxpy.OPTIMAL + or problem.status == cvxpy.OPTIMAL_INACCURATE + ): return np.array(ux.value).flatten() else: res = np.empty(self.get_no_vars()) res[:] = np.nan return res - diff --git a/toppra/solverwrapper/ecos_solverwrapper.py b/toppra/solverwrapper/ecos_solverwrapper.py index aa9355d1..5302d480 100644 --- a/toppra/solverwrapper/ecos_solverwrapper.py +++ b/toppra/solverwrapper/ecos_solverwrapper.py @@ -5,10 +5,12 @@ import numpy as np import scipy.sparse + logger = logging.getLogger(__name__) try: import ecos + IMPORT_ECOS = True except ImportError as err: logger.warn("Unable to import ecos") @@ -43,10 +45,13 @@ class ecosWrapper(SolverWrapper): The discretization grid use to discretize the geometric path. """ + def __init__(self, constraint_list, path, path_discretization): super(ecosWrapper, self).__init__(constraint_list, path, path_discretization) if not IMPORT_ECOS: - raise RuntimeError("Unable to start ecos wrapper because ECOS solver is not installed.") + raise RuntimeError( + "Unable to start ecos wrapper because ECOS solver is not installed." + ) # NOTE: Currently receive params in dense form. self._linear_idx = [] self._conic_idx = [] @@ -73,12 +78,18 @@ def __init__(self, constraint_list, path, path_discretization): self._conic_idx.append(i) self._conic_dim += 4 * self.params[i][0].shape[1] else: - raise NotImplementedError("Constraint type {:} not implemented".format(_type)) + raise NotImplementedError( + "Constraint type {:} not implemented".format(_type) + ) assert self._conic_dim % 4 == 0 logger.debug("Indices of linear constraints: {:}".format(self._linear_idx)) logger.debug("Indices of conic constraints : {:}".format(self._conic_idx)) - logger.debug("Nb of row for linear constraints: {:d} rows".format(self._linear_dim)) - logger.debug("Nb of row for conic constraints : {:d} rows".format(self._conic_dim)) + logger.debug( + "Nb of row for linear constraints: {:d} rows".format(self._linear_dim) + ) + logger.debug( + "Nb of row for conic constraints : {:d} rows".format(self._conic_dim) + ) def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): assert i <= self.N and 0 <= i @@ -101,9 +112,9 @@ def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): # Fill G and h currow = 0 ## Fill 1) - G_lil[currow: currow + 2, 1] = [[-1], [1]] + G_lil[currow : currow + 2, 1] = [[-1], [1]] if not np.isnan(x_min): - h[currow] = - x_min + h[currow] = -x_min else: h[currow] = ECOS_INFTY currow += 1 @@ -115,9 +126,9 @@ def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): ## Fill 2) if i < self.N: delta = self.get_deltas()[i] - G_lil[currow, :] = [[- 2 * delta, -1]] + G_lil[currow, :] = [[-2 * delta, -1]] if not np.isnan(x_next_min): - h[currow] = - x_next_min + h[currow] = -x_next_min else: h[currow] = ECOS_INFTY currow += 1 @@ -134,27 +145,35 @@ def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): if _a is not None: if self.constraints[k].identical: nb_cnst = _F.shape[0] - G_lil[currow:currow + nb_cnst, 0] = np.dot(_F, _a[i]).reshape(-1, 1) - G_lil[currow:currow + nb_cnst, 1] = np.dot(_F, _b[i]).reshape(-1, 1) - h[currow:currow + nb_cnst] = _h - np.dot(_F, _c[i]) + G_lil[currow : currow + nb_cnst, 0] = np.dot(_F, _a[i]).reshape( + -1, 1 + ) + G_lil[currow : currow + nb_cnst, 1] = np.dot(_F, _b[i]).reshape( + -1, 1 + ) + h[currow : currow + nb_cnst] = _h - np.dot(_F, _c[i]) currow += nb_cnst else: nb_cnst = _F.shape[1] - G_lil[currow:currow + nb_cnst, 0] = np.dot(_F[i], _a[i]).reshape(-1, 1) - G_lil[currow:currow + nb_cnst, 1] = np.dot(_F[i], _b[i]).reshape(-1, 1) - h[currow:currow + nb_cnst] = _h[i] - np.dot(_F[i], _c[i]) + G_lil[currow : currow + nb_cnst, 0] = np.dot(_F[i], _a[i]).reshape( + -1, 1 + ) + G_lil[currow : currow + nb_cnst, 1] = np.dot(_F[i], _b[i]).reshape( + -1, 1 + ) + h[currow : currow + nb_cnst] = _h[i] - np.dot(_F[i], _c[i]) currow += nb_cnst if _ubound is not None: G_lil[currow, 0] = 1 G_lil[currow + 1, 0] = -1 - h[currow: currow + 2] = [_ubound[i, 1], -_ubound[i, 0]] + h[currow : currow + 2] = [_ubound[i, 1], -_ubound[i, 0]] currow += 2 if _xbound is not None: G_lil[currow, 1] = 1 G_lil[currow + 1, 1] = -1 - h[currow: currow + 2] = [min(ECOS_MAXX, _xbound[i, 1]), -_xbound[i, 0]] + h[currow : currow + 2] = [min(ECOS_MAXX, _xbound[i, 1]), -_xbound[i, 0]] currow += 2 ## Fill 4) for k in self._conic_idx: @@ -167,26 +186,26 @@ def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): # [- P_ij^T[:, 2] ] for j in range(_a.shape[1]): G_lil[currow, :] = [_a[i, j], _b[i, j]] - G_lil[currow + 1: currow + 4, :] = _P[i, j].T[:, :2] - h[currow] = - _c[i, j] - h[currow + 1: currow + 4] = _P[i, j].T[:, 2] + G_lil[currow + 1 : currow + 4, :] = _P[i, j].T[:, :2] + h[currow] = -_c[i, j] + h[currow + 1 : currow + 4] = _P[i, j].T[:, 2] currow += 4 - # Fill + # Fill G = scipy.sparse.csc_matrix(G_lil) result = ecos.solve(g, G, h, dims, verbose=False) accepted_infos = ["Optimal solution found", "Close to optimal solution found"] - if result['info']['infostring'] in accepted_infos: + if result["info"]["infostring"] in accepted_infos: success = True else: success = False - logger.warning("Optimization fails. Result dictionary: \n {:}".format(result)) + logger.warning( + "Optimization fails. Result dictionary: \n {:}".format(result) + ) ux_opt = np.zeros(2) if success: - ux_opt = result['x'] + ux_opt = result["x"] else: ux_opt[:] = np.nan return ux_opt - - diff --git a/toppra/solverwrapper/hot_qpoases_solverwrapper.py b/toppra/solverwrapper/hot_qpoases_solverwrapper.py index 7bad2b2b..4691513f 100644 --- a/toppra/solverwrapper/hot_qpoases_solverwrapper.py +++ b/toppra/solverwrapper/hot_qpoases_solverwrapper.py @@ -2,10 +2,17 @@ import numpy as np from ..constraint import ConstraintType from ..constants import QPOASES_INFTY, TINY, SMALL +from ..exceptions import SolverNotFound + try: - from qpoases import (PyOptions as Options, PyPrintLevel as PrintLevel, - PyReturnValue as ReturnValue, PySQProblem as SQProblem) + from qpoases import ( + PyOptions as Options, + PyPrintLevel as PrintLevel, + PyReturnValue as ReturnValue, + PySQProblem as SQProblem, + ) + qpoases_FOUND = True except ImportError: qpoases_FOUND = False @@ -46,9 +53,20 @@ class hotqpOASESSolverWrapper(SolverWrapper): scaling_solverwrapper: bool, optional If is True, try to scale the data of each optimization before running. """ - def __init__(self, constraint_list, path, path_discretization, disable_check=False, scaling_solverwrapper=True): - assert qpoases_FOUND, "toppra is unable to find any installation of qpoases!" - super(hotqpOASESSolverWrapper, self).__init__(constraint_list, path, path_discretization) + + def __init__( + self, + constraint_list, + path, + path_discretization, + disable_check=False, + # scaling_solverwrapper=True, + ): + if not qpoases_FOUND: + SolverNotFound("toppra is unable to find any installation of qpoases!") + super(hotqpOASESSolverWrapper, self).__init__( + constraint_list, path, path_discretization + ) self._disable_check = disable_check # First constraint is x + 2 D u <= xnext_max, second is xnext_min <= x + 2D u @@ -67,9 +85,9 @@ def __init__(self, constraint_list, path, path_discretization, disable_check=Fal # l <= var <= h # lA <= A var <= hA self._A = np.zeros((self.nC, self.nV)) - self._lA = - np.ones(self.nC) * QPOASES_INFTY + self._lA = -np.ones(self.nC) * QPOASES_INFTY self._hA = np.ones(self.nC) * QPOASES_INFTY - self._l = - np.ones(2) * QPOASES_INFTY + self._l = -np.ones(2) * QPOASES_INFTY self._h = np.ones(2) * QPOASES_INFTY def setup_solver(self): @@ -101,7 +119,7 @@ def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): # s.t lA <= A scale y <= hA # l <= scale y <= h - self._l[:] = - QPOASES_INFTY + self._l[:] = -QPOASES_INFTY self._h[:] = QPOASES_INFTY if x_min is not None: @@ -113,7 +131,7 @@ def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): delta = self.get_deltas()[i] if x_next_min is not None: self._A[0] = [-2 * delta, -1] - self._hA[0] = - x_next_min + self._hA[0] = -x_next_min else: self._A[0] = [0, 0] self._hA[0] = QPOASES_INFTY @@ -132,16 +150,16 @@ def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): if a is not None: if self.constraints[j].identical: nC_ = F.shape[0] - self._A[cur_index: cur_index + nC_, 0] = F.dot(a[i]) - self._A[cur_index: cur_index + nC_, 1] = F.dot(b[i]) - self._hA[cur_index: cur_index + nC_] = v - F.dot(c[i]) - self._lA[cur_index: cur_index + nC_] = - QPOASES_INFTY + self._A[cur_index : cur_index + nC_, 0] = F.dot(a[i]) + self._A[cur_index : cur_index + nC_, 1] = F.dot(b[i]) + self._hA[cur_index : cur_index + nC_] = v - F.dot(c[i]) + self._lA[cur_index : cur_index + nC_] = -QPOASES_INFTY else: nC_ = F[i].shape[0] - self._A[cur_index: cur_index + nC_, 0] = F[i].dot(a[i]) - self._A[cur_index: cur_index + nC_, 1] = F[i].dot(b[i]) - self._hA[cur_index: cur_index + nC_] = v[i] - F[i].dot(c[i]) - self._lA[cur_index: cur_index + nC_] = - QPOASES_INFTY + self._A[cur_index : cur_index + nC_, 0] = F[i].dot(a[i]) + self._A[cur_index : cur_index + nC_, 1] = F[i].dot(b[i]) + self._hA[cur_index : cur_index + nC_] = v[i] - F[i].dot(c[i]) + self._lA[cur_index : cur_index + nC_] = -QPOASES_INFTY cur_index = cur_index + nC_ if ubound is not None: self._l[0] = max(self._l[0], ubound[i, 0]) @@ -154,16 +172,22 @@ def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): # if x_min == x_max, do not solve the 2D linear program, instead, do a line search if abs(x_min - x_max) < TINY and H is None and self.get_no_vars() == 2: logger.debug("x_min ({:f}) equals x_max ({:f})".format(x_min, x_max)) - u_min = - QPOASES_INFTY + u_min = -QPOASES_INFTY u_max = QPOASES_INFTY for i in range(self._A.shape[0]): if self._A[i, 0] > 0: - u_max = min(u_max, (self._hA[i] - self._A[i, 1] * x_min) / self._A[i, 0]) + u_max = min( + u_max, (self._hA[i] - self._A[i, 1] * x_min) / self._A[i, 0] + ) elif self._A[i, 0] < 0: - u_min = max(u_min, (self._hA[i] - self._A[i, 1] * x_min) / self._A[i, 0]) + u_min = max( + u_min, (self._hA[i] - self._A[i, 1] * x_min) / self._A[i, 0] + ) if (u_min - u_max) / abs(u_max) > SMALL: # problem infeasible - logger.debug("u_min > u_max by {:f}. Might not be critical. " - "Returning failure.".format(u_min - u_max)) + logger.debug( + "u_min > u_max by {:f}. Might not be critical. " + "Returning failure.".format(u_min - u_max) + ) return np.array([np.nan, np.nan]) if g[0] < 0: @@ -172,23 +196,29 @@ def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): return np.array([u_min, x_min + 2 * u_min * delta]) if H is None: - H = np.ones((self.get_no_vars(), self.get_no_vars())) * 1e-18 # regularization, very important + H = ( + np.ones((self.get_no_vars(), self.get_no_vars())) * 1e-18 + ) # regularization, very important - ratio_col1 = 1 / (np.sum(np.abs(self._A[2:, 0])) + 1e-5) # the maximum possible value for both ratios is 100000 + ratio_col1 = 1 / ( + np.sum(np.abs(self._A[2:, 0])) + 1e-5 + ) # the maximum possible value for both ratios is 100000 ratio_col2 = 1 / (np.sum(np.abs(self._A[2:, 1])) + 1e-5) variable_scales = np.array([ratio_col1, ratio_col2]) # variable_scales = np.array([5000.0, 2000.0]) variable_scales_mat = np.diag(variable_scales) if logger.isEnabledFor(logging.DEBUG): - logger.debug("min ratio col 1 {:f}, col 2 {:f}".format(ratio_col1, ratio_col2)) + logger.debug( + "min ratio col 1 {:f}, col 2 {:f}".format(ratio_col1, ratio_col2) + ) # ratio scaling self._A = self._A.dot(variable_scales_mat) self._l = self._l / variable_scales self._h = self._h / variable_scales - g = g * variable_scales - H = variable_scales_mat.dot(H).dot(variable_scales_mat) + self._g = g * variable_scales + self._H = variable_scales_mat.dot(H).dot(variable_scales_mat) # rows scaling row_magnitude = np.sum(np.abs(self._A), axis=1) @@ -197,35 +227,9 @@ def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): self._lA = np.dot(row_scaling_mat, self._lA) self._hA = np.dot(row_scaling_mat, self._hA) - # Select what solver to use - if g[1] > 0: # Choose solver_minimizing - if abs(self.solver_minimizing_recent_index - i) > 1: - if logger.isEnabledFor(logging.DEBUG): - logger.debug("solver_minimizing [init]") - res = self.solver_minimizing.init(H, g, self._A, self._l, self._h, self._lA, self._hA, np.array([1000])) - else: - if logger.isEnabledFor(logging.DEBUG): - logger.debug("solver_minimizing [hotstart]") - res = self.solver_minimizing.hotstart(H, g, self._A, self._l, self._h, self._lA, self._hA, np.array([1000])) - self.solver_minimizing_recent_index = i - else: # Choose solver_maximizing - if abs(self.solver_maximizing_recent_index - i) > 1: - if logger.isEnabledFor(logging.DEBUG): - logger.debug("solver_maximizing [init]") - res = self.solver_maximizing.init(H, g, self._A, self._l, self._h, self._lA, self._hA, np.array([1000])) - else: - if logger.isEnabledFor(logging.DEBUG): - logger.debug("solver_maximizing [hotstart]") - res = self.solver_maximizing.hotstart(H, g, self._A, self._l, self._h, self._lA, self._hA, np.array([1000])) - self.solver_maximizing_recent_index = i - - if res == ReturnValue.SUCCESSFUL_RETURN: - var = np.zeros(self.nV) - if g[1] > 0: - self.solver_minimizing.getPrimalSolution(var) - else: - self.solver_maximizing.getPrimalSolution(var) + return_value, var = self._solve_optimization(i) + if return_value == ReturnValue.SUCCESSFUL_RETURN: if logger.isEnabledFor(logging.DEBUG): logger.debug("optimal value: {:}".format(var)) @@ -233,30 +237,105 @@ def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): return var * variable_scales # Check for constraint feasibility - success = (np.all(self._l <= var + TINY) and np.all(var <= self._h + TINY) - and np.all(np.dot(self._A, var) <= self._hA + TINY) - and np.all(np.dot(self._A, var) >= self._lA - TINY)) + success = ( + np.all(self._l <= var + TINY) + and np.all(var <= self._h + TINY) + and np.all(np.dot(self._A, var) <= self._hA + TINY) + and np.all(np.dot(self._A, var) >= self._lA - TINY) + ) if not success: # import ipdb; ipdb.set_trace() - logger.fatal("Hotstart fails but qpOASES does not report correctly. \n " - "var: {:}, lower_bound: {:}, higher_bound{:}".format(var, self._l, self._h)) + logger.fatal( + "Hotstart fails but qpOASES does not report correctly. \n " + "var: {:}, lower_bound: {:}, higher_bound{:}".format( + var, self._l, self._h + ) + ) # TODO: Investigate why this happen and fix the # relevant code (in qpOASES wrapper) else: return var * variable_scales else: - logger.debug("Optimization fails. qpOASES error code: {:d}. Checking constraint feasibility for (0, 0)!".format(res)) - - if (np.all(0 <= self._hA) and np.all(0 >= self._lA) and np.all(0 <= self._h) and np.all(0 >= self._l)): - logger.fatal("(0, 0) satisfies all constraints => error due to numerical errors.") - print(self._A) - print(self._lA) - print(self._hA) - print(self._l) - print(self._h) + logger.debug("Optimization fails. qpOASES error code: %d.", return_value) + + if ( + np.all(0 <= self._hA) + and np.all(0 >= self._lA) + and np.all(0 <= self._h) + and np.all(0 >= self._l) + ): + logger.fatal( + "(0, 0) satisfies all constraints => error due to numerical errors.", + self._A, + self._lA, + self._hA, + self._l, + self._h, + ) else: logger.debug("(0, 0) does not satisfy all constraints.") - res = np.empty(self.get_no_vars()) - res[:] = np.nan - return res + return_value = np.empty(self.get_no_vars()) + return_value[:] = np.nan + return return_value + + def _solve_optimization(self, i): + var = np.zeros(self.nV) + if self._g[1] > 0: # Choose solver_minimizing + if abs(self.solver_minimizing_recent_index - i) > 1: + logger.debug("solver_minimizing [init]") + return_value = self.solver_minimizing.init( + self._H, + self._g, + self._A, + self._l, + self._h, + self._lA, + self._hA, + np.array([1000]), + ) + else: + if logger.isEnabledFor(logging.DEBUG): + logger.debug("solver_minimizing [hotstart]") + return_value = self.solver_minimizing.hotstart( + self._H, + self._g, + self._A, + self._l, + self._h, + self._lA, + self._hA, + np.array([1000]), + ) + self.solver_minimizing_recent_index = i + self.solver_minimizing.getPrimalSolution(var) + else: # Choose solver_maximizing + if abs(self.solver_maximizing_recent_index - i) > 1: + if logger.isEnabledFor(logging.DEBUG): + logger.debug("solver_maximizing [init]") + return_value = self.solver_maximizing.init( + self._H, + self._g, + self._A, + self._l, + self._h, + self._lA, + self._hA, + np.array([1000]), + ) + else: + if logger.isEnabledFor(logging.DEBUG): + logger.debug("solver_maximizing [hotstart]") + return_value = self.solver_maximizing.hotstart( + self._H, + self._g, + self._A, + self._l, + self._h, + self._lA, + self._hA, + np.array([1000]), + ) + self.solver_maximizing_recent_index = i + self.solver_maximizing.getPrimalSolution(var) + return return_value, var diff --git a/toppra/solverwrapper/qpoases_solverwrapper.py b/toppra/solverwrapper/qpoases_solverwrapper.py index e656bcd1..9587d877 100644 --- a/toppra/solverwrapper/qpoases_solverwrapper.py +++ b/toppra/solverwrapper/qpoases_solverwrapper.py @@ -2,9 +2,15 @@ import numpy as np from ..constraint import ConstraintType from ..constants import INFTY + try: - from qpoases import (PyOptions as Options, PyPrintLevel as PrintLevel, - PyReturnValue as ReturnValue, PySQProblem as SQProblem) + from qpoases import ( + PyOptions as Options, + PyPrintLevel as PrintLevel, + PyReturnValue as ReturnValue, + PySQProblem as SQProblem, + ) + qpoases_FOUND = True except ImportError: qpoases_FOUND = False @@ -27,10 +33,12 @@ class qpOASESSolverWrapper(SolverWrapper): def __init__(self, constraint_list, path, path_discretization): assert qpoases_FOUND, "toppra is unable to find any installation of qpoases!" - super(qpOASESSolverWrapper, self).__init__(constraint_list, path, path_discretization) + super(qpOASESSolverWrapper, self).__init__( + constraint_list, path, path_discretization + ) # Currently only support Canonical Linear Constraint - self.nC = 2 # First constraint is x + 2 D u <= xnext_max, second is xnext_min <= x + 2D u + self.nC = 2 # First constraint is x + 2 D u <= xnext_max, second is xnext_min <= x + 2D u for i, constraint in enumerate(constraint_list): if constraint.get_constraint_type() != ConstraintType.CanonicalLinear: raise NotImplementedError @@ -42,8 +50,8 @@ def __init__(self, constraint_list, path, path_discretization): self.nC += F.shape[1] self._A = np.zeros((self.nC, self.nV)) - self._lA = - np.ones(self.nC) - self._hA = - np.ones(self.nC) + self._lA = -np.ones(self.nC) + self._hA = -np.ones(self.nC) option = Options() option.printLevel = PrintLevel.NONE @@ -57,7 +65,7 @@ def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): # l <= y <= h assert i <= self.N and 0 <= i - l = - np.ones(2) * INF + l = -np.ones(2) * INF h = np.ones(2) * INF if x_min is not None: @@ -69,8 +77,8 @@ def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): delta = self.get_deltas()[i] if x_next_min is not None: self._A[0] = [-2 * delta, -1] - self._hA[0] = - x_next_min - self._lA[0] = - INF + self._hA[0] = -x_next_min + self._lA[0] = -INF else: self._A[0] = [0, 0] self._hA[0] = INF @@ -78,7 +86,7 @@ def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): if x_next_max is not None: self._A[1] = [2 * delta, 1] self._hA[1] = x_next_max - self._lA[1] = - INF + self._lA[1] = -INF else: self._A[1] = [0, 0] self._hA[1] = INF @@ -91,16 +99,16 @@ def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): if a is not None: if self.constraints[j].identical: nC_ = F.shape[0] - self._A[cur_index: cur_index + nC_, 0] = F.dot(a[i]) - self._A[cur_index: cur_index + nC_, 1] = F.dot(b[i]) - self._hA[cur_index: cur_index + nC_] = v - F.dot(c[i]) - self._lA[cur_index: cur_index + nC_] = - INF + self._A[cur_index : cur_index + nC_, 0] = F.dot(a[i]) + self._A[cur_index : cur_index + nC_, 1] = F.dot(b[i]) + self._hA[cur_index : cur_index + nC_] = v - F.dot(c[i]) + self._lA[cur_index : cur_index + nC_] = -INF else: nC_ = F[i].shape[0] - self._A[cur_index: cur_index + nC_, 0] = F[i].dot(a[i]) - self._A[cur_index: cur_index + nC_, 1] = F[i].dot(b[i]) - self._hA[cur_index: cur_index + nC_] = v[i] - F[i].dot(c[i]) - self._lA[cur_index: cur_index + nC_] = - INF + self._A[cur_index : cur_index + nC_, 0] = F[i].dot(a[i]) + self._A[cur_index : cur_index + nC_, 1] = F[i].dot(b[i]) + self._hA[cur_index : cur_index + nC_] = v[i] - F[i].dot(c[i]) + self._lA[cur_index : cur_index + nC_] = -INF cur_index = cur_index + nC_ if ubound is not None: @@ -114,7 +122,9 @@ def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): if H is None: H = np.zeros((self.get_no_vars(), self.get_no_vars())) - res = self.solver.init(H, g, self._A, l, h, self._lA, self._hA, np.array([1000])) + res = self.solver.init( + H, g, self._A, l, h, self._lA, self._hA, np.array([1000]) + ) if res == ReturnValue.SUCCESSFUL_RETURN: var = np.zeros(self.nV) self.solver.getPrimalSolution(var) diff --git a/toppra/solverwrapper/solverwrapper.py b/toppra/solverwrapper/solverwrapper.py index 64a879d2..2138d383 100644 --- a/toppra/solverwrapper/solverwrapper.py +++ b/toppra/solverwrapper/solverwrapper.py @@ -10,25 +10,28 @@ def available_solvers(output_msg=True): """Check for available solvers.""" try: import ecos + IMPORT_ECOS = True except ImportError as err: IMPORT_ECOS = False try: import qpoases + IMPORT_QPOASES = True except ImportError as err: IMPORT_QPOASES = False try: import cvxpy + IMPORT_CVXPY = True except ImportError as err: IMPORT_CVXPY = False solver_availability = ( - ('seidel', True), - ('hotqpoases', IMPORT_QPOASES), - ('qpoases', IMPORT_QPOASES), - ('ecos', IMPORT_ECOS), - ('cvxpy', IMPORT_CVXPY) + ("seidel", True), + ("hotqpoases", IMPORT_QPOASES), + ("qpoases", IMPORT_QPOASES), + ("ecos", IMPORT_ECOS), + ("cvxpy", IMPORT_CVXPY), ) if output_msg: @@ -79,14 +82,19 @@ def __init__(self, constraint_list, path, path_discretization): # problem will be solved as if the input path is scaled linearly. self.scaling = self.path_discretization[-1] / self.path.duration # End main attributes - self.N = len(path_discretization) - 1 # Number of stages. Number of point is _N + 1 + self.N = ( + len(path_discretization) - 1 + ) # Number of stages. Number of point is _N + 1 self.deltas = self.path_discretization[1:] - self.path_discretization[:-1] for i in range(self.N): assert path_discretization[i + 1] > path_discretization[i] self.params = [ - c.compute_constraint_params(self.path, self.path_discretization, self.scaling) - for c in self.constraints] + c.compute_constraint_params( + self.path, self.path_discretization, self.scaling + ) + for c in self.constraints + ] self.nV = 2 + sum([c.get_no_extra_vars() for c in self.constraints]) def get_no_stages(self): diff --git a/toppra/utils.py b/toppra/utils.py index 820b102f..2a2f9c7a 100644 --- a/toppra/utils.py +++ b/toppra/utils.py @@ -21,9 +21,14 @@ def deprecated(func): # pylint: disable=C0111 @functools.wraps(func) def new_func(*args, **kwargs): - warnings.warn("Call to deprecated function {} in module {}.".format( - func.__name__, func.__module__), category=DeprecationWarning) + warnings.warn( + "Call to deprecated function {} in module {}.".format( + func.__name__, func.__module__ + ), + category=DeprecationWarning, + ) return func(*args, **kwargs) + return new_func @@ -31,11 +36,13 @@ def setup_logging(level="WARN"): """Setup basic logging facility to console. """ coloredlogs.install( - logger=logging.getLogger("toppra"), level=level, + logger=logging.getLogger("toppra"), + level=level, fmt="%(levelname)5s [%(filename)s : %(lineno)d] [%(funcName)s] %(message)s", datefmt="%H:%M:%S", - milliseconds=True) - logging.basicConfig(filename='/tmp/toppra.log', level=level, filemode='a') + milliseconds=True, + ) + logging.basicConfig(filename="/tmp/toppra.log", level=level, filemode="a") def compute_jacobian_wrench(robot, link, point): @@ -101,7 +108,8 @@ def inv_dyn(rave_robot, q, qd, qdd, forceslist=None, returncomponents=True): rave_robot.SetDOFValues(_q) rave_robot.SetDOFVelocities(_qd) res = rave_robot.ComputeInverseDynamics( - _qdd, forceslist, returncomponents=returncomponents) + _qdd, forceslist, returncomponents=returncomponents + ) # Restore kinematic limits rave_robot.SetDOFVelocityLimits(vlim) rave_robot.SetDOFAccelerationLimits(alim) @@ -159,18 +167,22 @@ def smooth_singularities(parametrization_instance, us, xs, vs=None): for index in singular_indices: idstart = max(0, index) idend = min(parametrization_instance.N, index + 4) - xs_smth[range(idstart, idend + 1)] = ( - xs_smth[idstart] + (xs_smth[idend] - xs_smth[idstart]) * - np.linspace(0, 1, idend + 1 - idstart)) + xs_smth[range(idstart, idend + 1)] = xs_smth[idstart] + ( + xs_smth[idend] - xs_smth[idstart] + ) * np.linspace(0, 1, idend + 1 - idstart) if vs is not None: - data = [vs_smth[idstart] + - (xs_smth[idend] - xs_smth[idstart]) * frac - for frac in np.linspace(0, 1, idend + 1 - idstart)] + data = [ + vs_smth[idstart] + (xs_smth[idend] - xs_smth[idstart]) * frac + for frac in np.linspace(0, 1, idend + 1 - idstart) + ] vs_smth[range(idstart, idend + 1)] = np.array(data) for i in range(parametrization_instance.N): - us_smth[i] = ((xs_smth[i + 1] - xs_smth[i]) / 2 - / (parametrization_instance.ss[i + 1] - parametrization_instance.ss[i])) + us_smth[i] = ( + (xs_smth[i + 1] - xs_smth[i]) + / 2 + / (parametrization_instance.ss[i + 1] - parametrization_instance.ss[i]) + ) if vs is not None: return us_smth, xs_smth, vs_smth