├── .github └── workflows │ ├── main.yml │ ├── pull_request.yml │ └── release.yml ├── .gitignore ├── .pylintrc ├── .vscode ├── c_cpp_properties.json ├── generate_docs_api.sh ├── settings.json └── tasks.json ├── CITATION.cff ├── CMakeLists.txt ├── LICENSE ├── README.md ├── bin ├── before_install_centos.sh ├── before_install_ubuntu.sh ├── build.sh └── gen_stubs.sh ├── docs ├── Makefile ├── conf.py ├── index.rst ├── make.bat ├── panda_py.cli.rst ├── panda_py.constants.rst ├── panda_py.controllers.rst ├── panda_py.libfranka.rst ├── panda_py.motion.rst └── panda_py.rst ├── examples ├── cartesian_impedance.py ├── communication_test.py ├── mmc.py ├── notebooks │ ├── benchmark.ipynb │ ├── motion.ipynb │ └── tutorial.ipynb ├── pilot_buttons.py ├── teaching.py └── vacuum_object.py ├── include ├── constants.h ├── controllers │ ├── applied_force.h │ ├── applied_torque.h │ ├── cartesian_impedance.h │ ├── cartesian_trajectory.h │ ├── controller.h │ ├── force.h │ ├── integrated_velocity.h │ ├── joint_limits │ │ ├── virtual_wall.h │ │ └── virtual_wall_controller.h │ ├── joint_position.h │ └── joint_trajectory.h ├── kinematics │ ├── fk.h │ └── ik.h ├── motion │ ├── generators.h │ └── time_optimal │ │ ├── path.h │ │ └── trajectory.h ├── panda.h └── utils.h ├── logo.png ├── pyproject.toml └── src ├── _core.cpp ├── controllers ├── applied_force.cpp ├── applied_torque.cpp ├── cartesian_impedance.cpp ├── cartesian_trajectory.cpp ├── force.cpp ├── integrated_velocity.cpp ├── joint_limits │ └── virtual_wall.cpp ├── joint_position.cpp └── joint_trajectory.cpp ├── libfranka.cpp ├── motion ├── generators.cpp └── time_optimal │ ├── path.cpp │ └── trajectory.cpp ├── panda.cpp └── panda_py ├── __init__.py ├── __init__.pyi ├── _core.pyi ├── cli.py ├── constants.py ├── controllers.py ├── libfranka.pyi ├── motion.py └── py.typed /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | name: Check commit 2 | 3 | permissions: 4 | contents: write 5 | id-token: write 6 | pages: write 7 | 8 | on: 9 | push: 10 | branches: ["main"] 11 | 12 | jobs: 13 | build_wheels: 14 | name: Build wheels for libfranka ${{ matrix.libfranka-version }} 15 | runs-on: ubuntu-latest 16 | strategy: 17 | matrix: 18 | libfranka-version: ["0.7.1", "0.8.0", "0.9.2", "0.13.3"] 19 | steps: 20 | - uses: actions/checkout@v3 21 | - uses: actions/setup-python@v3 22 | - name: Install cibuildwheel 23 | run: python -m pip install cibuildwheel==2.16.2 24 | - name: Build using cibuildwheel 25 | run: ./bin/build.sh "${{ matrix.libfranka-version }}" 26 | documentation: 27 | name: "Build and deploy documentation" 28 | needs: [build_wheels] 29 | runs-on: ubuntu-latest 30 | env: 31 | LIBFRANKA_VER: "0.9.2" 32 | steps: 33 | - uses: actions/checkout@v3 34 | - uses: actions/setup-python@v3 35 | - name: Install requirements 36 | run: ./bin/before_install_ubuntu.sh 37 | - name: Build package 38 | run: pip install .[docs] 39 | - name: Build documentation 40 | run: cd docs && make html 41 | - name: Setup Pages 42 | uses: actions/configure-pages@v5 43 | - name: Upload artifact 44 | uses: actions/upload-pages-artifact@v3 45 | with: 46 | path: ./docs/_build/html 47 | - name: Deploy to GitHub Pages 48 | id: deployment 49 | uses: actions/deploy-pages@v4 50 | -------------------------------------------------------------------------------- /.github/workflows/pull_request.yml: -------------------------------------------------------------------------------- 1 | name: Check pull request 2 | 3 | on: 4 | pull_request: 5 | 6 | jobs: 7 | build_wheels: 8 | name: Build wheels for libfranka ${{ matrix.libfranka-version }} 9 | runs-on: ubuntu-latest 10 | strategy: 11 | matrix: 12 | libfranka-version: ["0.7.1", "0.8.0", "0.9.2", "0.13.3"] 13 | steps: 14 | - uses: actions/checkout@v3 15 | - uses: actions/setup-python@v3 16 | - name: Install cibuildwheel 17 | run: python -m pip install cibuildwheel==2.16.2 18 | - name: Build using cibuildwheel 19 | run: ./bin/build.sh "${{ matrix.libfranka-version }}" 20 | -------------------------------------------------------------------------------- /.github/workflows/release.yml: -------------------------------------------------------------------------------- 1 | name: Release 2 | 3 | on: 4 | release: 5 | types: [published] 6 | 7 | env: 8 | VERSION: 0.8.1 9 | 10 | permissions: write-all 11 | 12 | jobs: 13 | build_wheels: 14 | name: Build wheels for libfranka ${{ matrix.libfranka-version }} 15 | runs-on: ubuntu-latest 16 | strategy: 17 | matrix: 18 | libfranka-version: ["0.7.1", "0.8.0", "0.9.2", "0.13.3"] 19 | steps: 20 | - uses: actions/checkout@v3 21 | - uses: actions/setup-python@v3 22 | - name: Install cibuildwheel 23 | run: python -m pip install cibuildwheel==2.16.2 24 | - name: Build using cibuildwheel 25 | run: ./bin/build.sh "${{ matrix.libfranka-version }}" 26 | - uses: actions/upload-artifact@v3 27 | with: 28 | path: ./archive/panda_py_${{ env.VERSION }}_libfranka_${{ matrix.libfranka-version }}.zip 29 | name: zip-${{ matrix.libfranka-version }} 30 | upload_pypi: 31 | name: "Upload wheels to pypi" 32 | needs: [build_wheels] 33 | runs-on: ubuntu-latest 34 | env: 35 | LIBFRANKA_VER: "0.9.2" 36 | steps: 37 | - uses: actions/checkout@v3 38 | - uses: actions/setup-python@v3 39 | - name: Install requirements 40 | run: | 41 | ./bin/before_install_ubuntu.sh 42 | python -m pip install cibuildwheel 43 | - name: Build using cibuildwheel 44 | run: python -m cibuildwheel --output-dir dist 45 | - uses: pypa/gh-action-pypi-publish@v1.5.0 46 | with: 47 | user: __token__ 48 | password: ${{ secrets.PYPI_API_TOKEN }} 49 | upload_assets: 50 | name: "Upload libfranka ${{ matrix.libfranka-version }} release assets" 51 | needs: [build_wheels] 52 | runs-on: ubuntu-latest 53 | strategy: 54 | matrix: 55 | libfranka-version: ["0.7.1", "0.8.0", "0.9.2", "0.13.3"] 56 | steps: 57 | - uses: actions/download-artifact@v3 58 | with: 59 | name: zip-${{ matrix.libfranka-version }} 60 | path: archive 61 | - uses: actions/upload-release-asset@v1 62 | with: 63 | upload_url: ${{ github.event.release.upload_url }} 64 | asset_path: ./archive/panda_py_${{ env.VERSION }}_libfranka_${{ matrix.libfranka-version }}.zip 65 | asset_name: panda_py_${{ env.VERSION }}_libfranka_${{ matrix.libfranka-version }}.zip 66 | asset_content_type: application/zip 67 | env: 68 | GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} 69 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | trinkgelage_old 2 | # Byte-compiled / optimized / DLL files 3 | __pycache__/ 4 | *.py[cod] 5 | *$py.class 6 | 7 | # C extensions 8 | *.so 9 | 10 | # Distribution / packaging 11 | .Python 12 | build/ 13 | develop-eggs/ 14 | dist/ 15 | downloads/ 16 | eggs/ 17 | .eggs/ 18 | lib/ 19 | lib64/ 20 | parts/ 21 | sdist/ 22 | var/ 23 | wheels/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | cover/ 54 | 55 | # Translations 56 | *.mo 57 | *.pot 58 | 59 | # Django stuff: 60 | *.log 61 | local_settings.py 62 | db.sqlite3 63 | db.sqlite3-journal 64 | 65 | # Flask stuff: 66 | instance/ 67 | .webassets-cache 68 | 69 | # Scrapy stuff: 70 | .scrapy 71 | 72 | # Sphinx documentation 73 | docs/_build/ 74 | 75 | # PyBuilder 76 | .pybuilder/ 77 | target/ 78 | 79 | # Jupyter Notebook 80 | .ipynb_checkpoints 81 | 82 | # IPython 83 | profile_default/ 84 | ipython_config.py 85 | 86 | # pyenv 87 | # For a library or package, you might want to ignore these files since the code is 88 | # intended to run in multiple environments; otherwise, check them in: 89 | # .python-version 90 | 91 | # pipenv 92 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 93 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 94 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 95 | # install all needed dependencies. 96 | #Pipfile.lock 97 | 98 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 99 | __pypackages__/ 100 | 101 | # Celery stuff 102 | celerybeat-schedule 103 | celerybeat.pid 104 | 105 | # SageMath parsed files 106 | *.sage.py 107 | 108 | # Environments 109 | .env 110 | .venv 111 | env/ 112 | venv/ 113 | ENV/ 114 | env.bak/ 115 | venv.bak/ 116 | 117 | # Spyder project settings 118 | .spyderproject 119 | .spyproject 120 | 121 | # Rope project settings 122 | .ropeproject 123 | 124 | # mkdocs documentation 125 | /site 126 | 127 | # mypy 128 | .mypy_cache/ 129 | .dmypy.json 130 | dmypy.json 131 | 132 | # Pyre type checker 133 | .pyre/ 134 | 135 | # pytype static type analyzer 136 | .pytype/ 137 | 138 | # Cython debug symbols 139 | cython_debug/ 140 | 141 | # setuptools_scm 142 | src/*/_version.py 143 | 144 | 145 | # ruff 146 | .ruff_cache/ 147 | 148 | # OS specific stuff 149 | .DS_Store 150 | .DS_Store? 151 | ._* 152 | .Spotlight-V100 153 | .Trashes 154 | ehthumbs.db 155 | Thumbs.db 156 | 157 | # Common editor files 158 | *~ 159 | *.swp 160 | -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Linux", 5 | "includePath": [ 6 | "${workspaceFolder}/**", 7 | "~/.local/lib/python3.10/site-packages/pybind11/include", 8 | "/usr/include/**" 9 | ], 10 | "defines": [], 11 | "compilerPath": "/usr/bin/gcc", 12 | "cStandard": "c17", 13 | "cppStandard": "gnu++17", 14 | "intelliSenseMode": "linux-gcc-x64" 15 | } 16 | ], 17 | "version": 4 18 | } -------------------------------------------------------------------------------- /.vscode/generate_docs_api.sh: -------------------------------------------------------------------------------- 1 | package_name="panda-python" 2 | 3 | if metadata=$(pip show "$package_name" 2>/dev/null); then 4 | if [[ $metadata == *"Editable"* ]]; then 5 | echo -e "\e[0;31mPackage '$package_name' is installed in editable mode. \e[0m" 6 | exit 1 7 | else 8 | location=$(pip show -f "$package_name" | awk '/^Location:/ {print $2}') 9 | sphinx-apidoc -e -d 1 -M -T -f -o docs $location/panda_python 10 | fi 11 | else 12 | echo -e "\e[0;31mPackage '$package_name' is not installed. \e[0m" 13 | exit 1 14 | fi 15 | -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "pylint.args": [ 3 | "--rcfile=.pylintrc" 4 | ], 5 | 6 | "yapf.args": [ 7 | "--style", 8 | "{based_on_style: google, indent_width: 2}" 9 | ], 10 | 11 | "[python]": { 12 | "editor.formatOnSaveMode": "file", 13 | "editor.formatOnSave": true, 14 | "editor.defaultFormatter": "eeyore.yapf", 15 | "editor.codeActionsOnSave": { 16 | "source.organizeImports": "explicit" 17 | }, 18 | }, 19 | "editor.tabSize": 2, 20 | "files.associations": { 21 | "cctype": "cpp", 22 | "clocale": "cpp", 23 | "cmath": "cpp", 24 | "cstdarg": "cpp", 25 | "cstddef": "cpp", 26 | "cstdio": "cpp", 27 | "cstdlib": "cpp", 28 | "cstring": "cpp", 29 | "ctime": "cpp", 30 | "cwchar": "cpp", 31 | "cwctype": "cpp", 32 | "array": "cpp", 33 | "atomic": "cpp", 34 | "bit": "cpp", 35 | "*.tcc": "cpp", 36 | "chrono": "cpp", 37 | "compare": "cpp", 38 | "complex": "cpp", 39 | "concepts": "cpp", 40 | "condition_variable": "cpp", 41 | "cstdint": "cpp", 42 | "deque": "cpp", 43 | "forward_list": "cpp", 44 | "list": "cpp", 45 | "map": "cpp", 46 | "set": "cpp", 47 | "string": "cpp", 48 | "unordered_map": "cpp", 49 | "unordered_set": "cpp", 50 | "vector": "cpp", 51 | "exception": "cpp", 52 | "algorithm": "cpp", 53 | "functional": "cpp", 54 | "iterator": "cpp", 55 | "memory": "cpp", 56 | "memory_resource": "cpp", 57 | "numeric": "cpp", 58 | "optional": "cpp", 59 | "random": "cpp", 60 | "ratio": "cpp", 61 | "string_view": "cpp", 62 | "system_error": "cpp", 63 | "tuple": "cpp", 64 | "type_traits": "cpp", 65 | "utility": "cpp", 66 | "fstream": "cpp", 67 | "initializer_list": "cpp", 68 | "iosfwd": "cpp", 69 | "iostream": "cpp", 70 | "istream": "cpp", 71 | "limits": "cpp", 72 | "mutex": "cpp", 73 | "new": "cpp", 74 | "numbers": "cpp", 75 | "ostream": "cpp", 76 | "semaphore": "cpp", 77 | "sstream": "cpp", 78 | "stdexcept": "cpp", 79 | "stop_token": "cpp", 80 | "streambuf": "cpp", 81 | "thread": "cpp", 82 | "cinttypes": "cpp", 83 | "typeindex": "cpp", 84 | "typeinfo": "cpp", 85 | "valarray": "cpp", 86 | "variant": "cpp" 87 | }, 88 | } -------------------------------------------------------------------------------- /.vscode/tasks.json: -------------------------------------------------------------------------------- 1 | { 2 | // See https://go.microsoft.com/fwlink/?LinkId=733558 3 | // for the documentation about the tasks.json format 4 | "version": "2.0.0", 5 | "tasks": [ 6 | { 7 | "label": "pytest", 8 | "type": "shell", 9 | "command": "pytest --cov panda-py ./tests/" 10 | }, 11 | { 12 | "label": "pylint", 13 | "type": "shell", 14 | "command": "pylint --rcfile .pylintrc src/" 15 | }, 16 | { 17 | "label": "generate-stub", 18 | "type": "shell", 19 | "command": "pybind11-stubgen --no-setup-py --root-module-suffix \"\" -o ./src/ --log-level INFO panda_py" 20 | }, 21 | { 22 | "label": "build-doc", 23 | "type": "shell", 24 | "command": "cd docs && make clean && make html" 25 | }, 26 | { 27 | "label": "generate-doc-api", 28 | "type": "shell", 29 | "command": [".vscode/generate_docs_api.sh"] 30 | } 31 | ] 32 | } 33 | -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.0 2 | message: "If you use this software, please cite it as below." 3 | authors: 4 | - family-names: "Elsner" 5 | given-names: "Jean" 6 | orcid: "https://orcid.org/0000-0003-2691-0099" 7 | title: "panda-py" 8 | url: "https://github.com/JeanElsner/panda-py" 9 | preferred-citation: 10 | type: article 11 | authors: 12 | - family-names: "Elsner" 13 | given-names: "Jean" 14 | orcid: "https://orcid.org/0000-0003-2691-0099" 15 | doi: "https://doi.org/10.1016/j.softx.2023.101532" 16 | url: "https://www.sciencedirect.com/science/article/pii/S2352711023002285" 17 | journal: "SoftwareX" 18 | start: 101532 19 | title: "Taming the Panda with Python: A powerful duo for seamless robotics programming and integration" 20 | issn: "2352-7110" 21 | volume: 24 22 | year: 2023 -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.15...3.25) 2 | project( 3 | "panda_py" 4 | LANGUAGES CXX) 5 | # VERSION "${SKBUILD_PROJECT_VERSION}") 6 | 7 | set (CMAKE_CXX_STANDARD 17) 8 | set(CMAKE_BUILD_TYPE "Release") 9 | set(THREADS_PREFER_PTHREAD_FLAG ON) 10 | 11 | option(VACUUM_GRIPPER OFF) 12 | if (VACUUM_GRIPPER) 13 | add_definitions(-DVACUUM_GRIPPER=${VACUUM_GRIPPER}) 14 | endif() 15 | 16 | find_package(Eigen3 REQUIRED) 17 | find_package(Threads REQUIRED) 18 | find_package(Franka REQUIRED) 19 | find_package( 20 | Python 21 | COMPONENTS Interpreter Development.Module 22 | REQUIRED) 23 | find_package(pybind11 CONFIG REQUIRED) 24 | 25 | ## _core module 26 | pybind11_add_module(_core 27 | src/_core.cpp 28 | src/panda.cpp 29 | src/controllers/joint_limits/virtual_wall.cpp 30 | src/controllers/integrated_velocity.cpp 31 | src/controllers/joint_position.cpp 32 | src/controllers/cartesian_impedance.cpp 33 | src/controllers/applied_torque.cpp 34 | src/controllers/applied_force.cpp 35 | src/controllers/force.cpp 36 | src/controllers/joint_trajectory.cpp 37 | src/controllers/cartesian_trajectory.cpp 38 | src/motion/generators.cpp 39 | src/motion/time_optimal/trajectory.cpp 40 | src/motion/time_optimal/path.cpp 41 | ) 42 | 43 | target_link_libraries(_core PUBLIC 44 | Threads::Threads 45 | ${Franka_LIBRARIES} 46 | ) 47 | 48 | target_include_directories(_core SYSTEM PUBLIC 49 | include 50 | ${EIGEN3_INCLUDE_DIRS} 51 | ${Franka_INCLUDE_DIRS} 52 | ) 53 | 54 | #target_compile_definitions(_core 55 | # PRIVATE VERSION_INFO=${PROJECT_VERSION}) 56 | 57 | install(TARGETS _core LIBRARY DESTINATION panda_py) 58 | 59 | ## libfranka module 60 | pybind11_add_module(libfranka 61 | src/libfranka.cpp) 62 | 63 | target_link_libraries(libfranka PUBLIC 64 | ${Franka_LIBRARIES} 65 | ) 66 | 67 | target_include_directories(libfranka SYSTEM PUBLIC 68 | ${Franka_INCLUDE_DIRS} 69 | ) 70 | 71 | #target_compile_definitions(libfranka 72 | # PRIVATE VERSION_INFO=${PROJECT_VERSION}) 73 | 74 | install(TARGETS libfranka LIBRARY DESTINATION panda_py) 75 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |
panda-py Logo
2 | 3 |

panda-py

4 | 5 |

6 | Continuous Integration 7 | Apache-2.0 License 8 | PyPI - Published Version 9 | PyPI - Python Version 10 | Documentation 11 |

12 | 13 | Finally, Python bindings for the Panda. These will increase your productivity by 1000%, guaranteed[^1]! 14 | 15 | ## Getting started 16 | 17 | To get started, check out the [tutorial paper](https://www.sciencedirect.com/science/article/pii/S2352711023002285), Jupyter [notebooks](https://github.com/JeanElsner/panda-py/tree/main/examples/notebooks) and other examples you can run directly on your robot. For more details on the API, please refer to the [documentation](https://jeanelsner.github.io/panda-py/). 18 | 19 | ## Extensions 20 | 21 | * [dm_robotics_panda](https://github.com/JeanElsner/dm_robotics_panda) Panda model and tools for reinforcement learning in `dm_robotics` 22 | 23 | ## Install 24 | 25 | ``` 26 | pip install panda-python 27 | ``` 28 | 29 | This will install panda-py and all its requirements. The pip version ships with libfranka 0.9.2, the newest version for the Franka Emika Robot. Please refer to the section below if you use an older system version or the more recent Franka Research 3 robot. 30 | 31 | ## libfranka Version 32 | 33 | There are currently two robot models available from Franka Emika: the Franka Emika Robot (FER, formerly known as Panda) and the Franka Research 3 (FR3). Depending on the installed firmware, the FER supports version <0.10 while the FR3 requires version >=0.10. For details, refer to [this](https://frankaemika.github.io/docs/compatibility.html) compatibility matrix. If you need a libfranka version different from the default 0.9.2, download the respective zip archive from the [release page](https://github.com/JeanElsner/panda-py/releases). Extract the archive and install the wheel for your Python version with pip, e.g., run 34 | ``` 35 | pip install panda_python-*libfranka.0.7.1-cp310*.whl 36 | ``` 37 | to install the binary wheel for libfranka 0.7.1 and Python 3.10. 38 | 39 | # Citation 40 | 41 | If you use panda-py in published research, please consider citing the [original software paper](https://www.sciencedirect.com/science/article/pii/S2352711023002285). 42 | 43 | ``` 44 | @article{elsner2023taming, 45 | title = {Taming the Panda with Python: A powerful duo for seamless robotics programming and integration}, 46 | journal = {SoftwareX}, 47 | volume = {24}, 48 | pages = {101532}, 49 | year = {2023}, 50 | issn = {2352-7110}, 51 | doi = {https://doi.org/10.1016/j.softx.2023.101532}, 52 | url = {https://www.sciencedirect.com/science/article/pii/S2352711023002285}, 53 | author = {Jean Elsner} 54 | } 55 | ``` 56 | 57 | [^1]: Not actually guaranteed. Based on a sample size of one. 58 | -------------------------------------------------------------------------------- /bin/before_install_centos.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | sed -i 's/enabled=1/enabled=0/g' /etc/yum/pluginconf.d/fastestmirror.conf 4 | sed -i 's/^mirrorlist/#mirrorlist/g' /etc/yum.repos.d/*.repo 5 | sed -i 's;^.*baseurl=http://mirror;baseurl=https://vault;g' /etc/yum.repos.d/*.repo 6 | fixup-mirrors 7 | yum -y update 8 | fixup-mirrors 9 | 10 | yum install -y openssl-devel 11 | git clone https://github.com/pocoproject/poco.git 12 | cd poco 13 | git checkout poco-1.9.2-release # focal, jammy uses 1.11.0 14 | mkdir cmake-build 15 | cd cmake-build 16 | cmake \ 17 | -DENABLE_ENCODINGS=OFF \ 18 | -DENABLE_ENCODINGS_COMPILER=OFF \ 19 | -DENABLE_XML=ON \ 20 | -DENABLE_JSON=ON \ 21 | -DENABLE_MONGODB=OFF \ 22 | -DENABLE_DATA_SQLITE=OFF \ 23 | -DENABLE_REDIS=OFF \ 24 | -DENABLE_PDF=OFF \ 25 | -DENABLE_UTIL=ON \ 26 | -DENABLE_NET=ON \ 27 | -DENABLE_SEVENZIP=OFF \ 28 | -DENABLE_ZIP=OFF \ 29 | -DENABLE_CPPPARSER=OFF \ 30 | -DENABLE_POCODOC=OFF \ 31 | -DENABLE_PAGECOMPILER=OFF \ 32 | -DENABLE_PAGECOMPILER_FILE2PAGE=OFF \ 33 | -DENABLE_ACTIVERECORD=OFF \ 34 | -DENABLE_ACTIVERECORD_COMPILER=OFF .. 35 | cmake --build . --config Release 36 | cmake --build . --target install 37 | 38 | # For legacy versions, use my patched repository 39 | repo="https://github.com/frankaemika/libfranka.git" 40 | if [[ "$LIBFRANKA_VER" == "0.7.1" || "$LIBFRANKA_VER" == "0.8.0" ]]; then 41 | repo="https://github.com/JeanElsner/libfranka.git" 42 | fi 43 | 44 | yum install -y eigen3-devel boost-devel 45 | git clone --recursive $repo 46 | cd libfranka 47 | git checkout $LIBFRANKA_VER 48 | git submodule update 49 | mkdir build && cd build 50 | cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF -DBUILD_EXAMPLES=OFF .. 51 | cmake --build . 52 | make install 53 | -------------------------------------------------------------------------------- /bin/before_install_ubuntu.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | sudo apt install build-essential cmake git libpoco-dev libeigen3-dev 4 | 5 | # For legacy versions, use my patched repository 6 | repo="https://github.com/frankaemika/libfranka.git" 7 | if [[ "$LIBFRANKA_VER" == "0.7.1" || "$LIBFRANKA_VER" == "0.8.0" ]]; then 8 | repo="https://github.com/JeanElsner/libfranka.git" 9 | fi 10 | 11 | sudo apt remove "*libfranka*" 12 | git clone --recursive $repo 13 | cd libfranka 14 | git checkout $LIBFRANKA_VER 15 | git submodule update 16 | mkdir build && cd build 17 | cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF -DBUILD_EXAMPLES=OFF .. 18 | cmake --build . 19 | cpack -G DEB 20 | sudo dpkg -i libfranka*.deb 21 | -------------------------------------------------------------------------------- /bin/build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Check if the libfranka version is provided as an argument 4 | if [ -z "$1" ]; then 5 | echo "Usage: $0 " 6 | exit 1 7 | fi 8 | 9 | # Install Python dependencies 10 | python -m pip install packaging toml cibuildwheel 11 | 12 | # Set pyproject.toml path 13 | root=$(dirname $0)/.. 14 | toml="$root/pyproject.toml" 15 | 16 | # Store current version 17 | version=$(python <NUL 2>NUL 14 | if errorlevel 9009 ( 15 | echo. 16 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 17 | echo.installed, then set the SPHINXBUILD environment variable to point 18 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 19 | echo.may add the Sphinx directory to PATH. 20 | echo. 21 | echo.If you don't have Sphinx installed, grab it from 22 | echo.https://www.sphinx-doc.org/ 23 | exit /b 1 24 | ) 25 | 26 | if "%1" == "" goto help 27 | 28 | %SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 29 | goto end 30 | 31 | :help 32 | %SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 33 | 34 | :end 35 | popd 36 | -------------------------------------------------------------------------------- /docs/panda_py.cli.rst: -------------------------------------------------------------------------------- 1 | panda\_py.cli module 2 | ==================== 3 | 4 | .. automodule:: panda_py.cli 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | -------------------------------------------------------------------------------- /docs/panda_py.constants.rst: -------------------------------------------------------------------------------- 1 | panda\_py.constants module 2 | ========================== 3 | 4 | .. automodule:: panda_py.constants 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | -------------------------------------------------------------------------------- /docs/panda_py.controllers.rst: -------------------------------------------------------------------------------- 1 | panda\_py.controllers module 2 | ============================ 3 | 4 | .. automodule:: panda_py.controllers 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | -------------------------------------------------------------------------------- /docs/panda_py.libfranka.rst: -------------------------------------------------------------------------------- 1 | panda\_py.libfranka module 2 | ========================== 3 | 4 | .. automodule:: panda_py.libfranka 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | -------------------------------------------------------------------------------- /docs/panda_py.motion.rst: -------------------------------------------------------------------------------- 1 | panda\_py.motion module 2 | ======================= 3 | 4 | .. automodule:: panda_py.motion 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | -------------------------------------------------------------------------------- /docs/panda_py.rst: -------------------------------------------------------------------------------- 1 | panda\_py package 2 | ================= 3 | 4 | .. automodule:: panda_py 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | 9 | Submodules 10 | ---------- 11 | 12 | .. toctree:: 13 | :maxdepth: 1 14 | 15 | panda_py.cli 16 | panda_py.constants 17 | panda_py.controllers 18 | panda_py.libfranka 19 | panda_py.motion 20 | -------------------------------------------------------------------------------- /examples/cartesian_impedance.py: -------------------------------------------------------------------------------- 1 | """ 2 | Uses the cartesian impedance controller to create a sinusoidal 3 | end-effector movement along the robot's y-axis. 4 | """ 5 | import sys 6 | 7 | import numpy as np 8 | 9 | import panda_py 10 | from panda_py import controllers 11 | 12 | if __name__ == '__main__': 13 | if len(sys.argv) < 2: 14 | raise RuntimeError(f'Usage: python {sys.argv[0]} ') 15 | 16 | panda = panda_py.Panda(sys.argv[1]) 17 | panda.move_to_start() 18 | ctrl = controllers.CartesianImpedance(filter_coeff=1.0) 19 | x0 = panda.get_position() 20 | q0 = panda.get_orientation() 21 | runtime = np.pi * 4.0 22 | panda.start_controller(ctrl) 23 | 24 | with panda.create_context(frequency=1e3, max_runtime=runtime) as ctx: 25 | while ctx.ok(): 26 | x_d = x0.copy() 27 | x_d[1] += 0.1 * np.sin(ctrl.get_time()) 28 | ctrl.set_control(x_d, q0) 29 | -------------------------------------------------------------------------------- /examples/communication_test.py: -------------------------------------------------------------------------------- 1 | """ 2 | Recreation of `communication_test.cpp` from libfranka using `panda_py`. 3 | 4 | An example indicating the network performance. 5 | 6 | @warning Before executing this example, make sure there is enough space in front of the robot. 7 | """ 8 | import sys 9 | 10 | import panda_py 11 | from panda_py import constants, controllers 12 | 13 | if __name__ == '__main__': 14 | if len(sys.argv) < 2: 15 | raise RuntimeError(f'Usage: python {sys.argv[0]} ') 16 | 17 | COUNTER = 0 18 | AVG_SUCCESS_RATE = 0.0 19 | MIN_SUCCESS_RATE = 1.0 20 | MAX_SUCCESS_RATE = 0.0 21 | TIME = 0 22 | 23 | panda = panda_py.Panda(sys.argv[1]) 24 | controller = controllers.AppliedTorque() 25 | # Sending zero torques - if EE is configured correctly, robot should not move 26 | panda.start_controller(controller) 27 | 28 | # Run a loop at 1kHz for 10 seconds. 29 | # This thread is decoupled from the control loop, 30 | # as such the number of lost packages is an estimate. 31 | with panda.create_context(frequency=1e3, max_runtime=10) as ctx: 32 | while ctx.ok(): 33 | success_rate = panda.get_state().control_command_success_rate 34 | if ctx.num_ticks % 100 == 0: 35 | print(f'#{ctx.num_ticks} Current success rate: {success_rate:.3f}') 36 | AVG_SUCCESS_RATE += success_rate 37 | if success_rate > MAX_SUCCESS_RATE: 38 | MAX_SUCCESS_RATE = success_rate 39 | if success_rate < MIN_SUCCESS_RATE and success_rate > 0: 40 | MIN_SUCCESS_RATE = success_rate 41 | panda.stop_controller() 42 | TIME = controller.get_time() 43 | COUNTER = ctx.num_ticks 44 | print('\nFinished test, shutting down example') 45 | 46 | AVG_SUCCESS_RATE /= COUNTER 47 | print('\n') 48 | print('#######################################################') 49 | lost_robot_states = TIME * 1000 * (1 - AVG_SUCCESS_RATE) 50 | if lost_robot_states > 0: 51 | print( 52 | f'The control loop did not get executed {lost_robot_states:.0f} times in the' 53 | ) 54 | print( 55 | f'last {round(TIME*1000)} milliseconds (lost {lost_robot_states:.0f}) robot states' 56 | ) 57 | print('\n') 58 | print(f'Control command success rate of {COUNTER} samples:') 59 | print(f'Max: {MAX_SUCCESS_RATE:.3f}') 60 | print(f'Avg: {AVG_SUCCESS_RATE:.3f}') 61 | print(f'Min: {MIN_SUCCESS_RATE:.3f}') 62 | if AVG_SUCCESS_RATE < 0.9: 63 | print('\nWARNING: THIS SETUP IS PROBABLY NOT SUFFICIENT FOR FCI!') 64 | print('PLEASE TRY OUT A DIFFERENT PC / NIC') 65 | if AVG_SUCCESS_RATE < 0.95: 66 | print('\nWARNING: MANY PACKETS GOT LOST!') 67 | print('PLEASE INSPECT YOUR SETUP AND FOLLOW ADVICE ON') 68 | print('https://frankaemika.github.io/docs/troubleshooting.html') 69 | print('#######################################################') 70 | -------------------------------------------------------------------------------- /examples/mmc.py: -------------------------------------------------------------------------------- 1 | """ 2 | This is an implementation of the controller from: 3 | J. Haviland and P. Corke, “A purely-reactive manipulability-maximising 4 | motion controller,” arXiv preprint arXiv:2002.11901,2020. 5 | """ 6 | import sys 7 | 8 | import numpy as np 9 | import qpsolvers as qp 10 | import roboticstoolbox as rtb 11 | import spatialmath as sm 12 | 13 | import panda_py 14 | from panda_py import controllers 15 | 16 | if __name__ == '__main__': 17 | if len(sys.argv) < 2: 18 | raise RuntimeError(f'Usage: python {sys.argv[0]} ') 19 | 20 | # Initialize robot hardware and controller 21 | ctrl = controllers.IntegratedVelocity() 22 | panda = panda_py.Panda(sys.argv[1]) 23 | panda.move_to_start() 24 | panda.start_controller(ctrl) 25 | 26 | # Initialize roboticstoolbox model 27 | panda_rtb = rtb.models.Panda() 28 | 29 | # Set the desired end-effector pose 30 | Tep = panda_rtb.fkine(panda.q) * sm.SE3(0.3, 0.2, 0.3) 31 | 32 | # Number of joint in the panda which we are controlling 33 | n = 7 34 | 35 | arrived = False 36 | 37 | with panda.create_context(frequency=20) as ctx: 38 | while ctx.ok() and not arrived: 39 | 40 | # The pose of the Panda's end-effector 41 | Te = panda_rtb.fkine(panda.q) 42 | 43 | # Transform from the end-effector to desired pose 44 | eTep = Te.inv() * Tep 45 | 46 | # Spatial error 47 | e = np.sum(np.abs(np.r_[eTep.t, eTep.rpy() * np.pi / 180])) 48 | 49 | # Calulate the required end-effector spatial velocity for the robot 50 | # to approach the goal. Gain is set to 1.0 51 | v, arrived = rtb.p_servo(Te, Tep, 1.0) 52 | 53 | # Gain term (lambda) for control minimisation 54 | Y = 0.01 55 | 56 | # Quadratic component of objective function 57 | Q = np.eye(n + 6) 58 | 59 | # Joint velocity component of Q 60 | Q[:n, :n] *= Y 61 | 62 | # Slack component of Q 63 | Q[n:, n:] = (1 / e) * np.eye(6) 64 | 65 | # The equality contraints 66 | Aeq = np.c_[panda_rtb.jacobe(panda.q), np.eye(6)] 67 | beq = v.reshape((6,)) 68 | 69 | # Linear component of objective function: the manipulability Jacobian 70 | c = np.r_[-panda_rtb.jacobm().reshape((n,)), np.zeros(6)] 71 | 72 | # The lower and upper bounds on the joint velocity and slack variable 73 | lb = -np.r_[panda_rtb.qdlim[:n], 10 * np.ones(6)] 74 | ub = np.r_[panda_rtb.qdlim[:n], 10 * np.ones(6)] 75 | 76 | # Solve for the joint velocities dq 77 | qd = qp.solve_qp(Q, c, None, None, Aeq, beq, lb=lb, ub=ub, solver='daqp') 78 | 79 | # Apply the joint velocities to the Panda 80 | ctrl.set_control(qd[:n]) 81 | -------------------------------------------------------------------------------- /examples/notebooks/benchmark.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "Benchmarks\n", 8 | "----------\n", 9 | "\n", 10 | "Import necessary libraries. We use ansitable to print a table of benchmarks and numpy for random state-space sampling." 11 | ] 12 | }, 13 | { 14 | "cell_type": "code", 15 | "execution_count": 21, 16 | "metadata": {}, 17 | "outputs": [], 18 | "source": [ 19 | "import time\n", 20 | "import panda_py\n", 21 | "from panda_py import constants, motion\n", 22 | "import numpy as np\n", 23 | "import ansitable" 24 | ] 25 | }, 26 | { 27 | "cell_type": "markdown", 28 | "metadata": {}, 29 | "source": [ 30 | "Each function is evaluated for `num_samples` number of samples. Trajectories consist of `num_waypoints` waypoints each. We randomly sample the state-space defined by the joint limits for joint positions stored in `q_rand` and store the corresponding end-effector poses in `T_rand`." 31 | ] 32 | }, 33 | { 34 | "cell_type": "code", 35 | "execution_count": 22, 36 | "metadata": {}, 37 | "outputs": [], 38 | "source": [ 39 | "num_samples = 10000\n", 40 | "num_waypoints = 5\n", 41 | "np.random.seed(0)\n", 42 | "q_rand = np.random.uniform(low=constants.JOINT_LIMITS_LOWER,\n", 43 | " high=constants.JOINT_LIMITS_UPPER,\n", 44 | " size=(num_samples, 7))\n", 45 | "T_rand = [panda_py.fk(q) for q in q_rand]" 46 | ] 47 | }, 48 | { 49 | "cell_type": "markdown", 50 | "metadata": {}, 51 | "source": [ 52 | "Execute and time the samples for inverse and forward kinematics as well joint and Cartesian trajectory generation. Total runtime is around 5 minutes for 10000 samples." 53 | ] 54 | }, 55 | { 56 | "cell_type": "code", 57 | "execution_count": 23, 58 | "metadata": {}, 59 | "outputs": [ 60 | { 61 | "name": "stderr", 62 | "output_type": "stream", 63 | "text": [ 64 | "Trajectory computation timed out after 1 seconds.\n", 65 | "Trajectory computation timed out after 1 seconds.\n", 66 | "Trajectory computation timed out after 1 seconds.\n", 67 | "Trajectory computation timed out after 1 seconds.\n", 68 | "Trajectory computation timed out after 1 seconds.\n" 69 | ] 70 | } 71 | ], 72 | "source": [ 73 | "ik = []\n", 74 | "for T in T_rand:\n", 75 | " t = time.time()\n", 76 | " panda_py.ik(T)\n", 77 | " ik.append(time.time() - t)\n", 78 | "\n", 79 | "fk = []\n", 80 | "for q in q_rand:\n", 81 | " t = time.time()\n", 82 | " panda_py.fk(q)\n", 83 | " fk.append(time.time() - t)\n", 84 | "\n", 85 | "joint_motion = []\n", 86 | "for i in range(num_samples):\n", 87 | " waypoints = []\n", 88 | " for k in range(num_waypoints):\n", 89 | " waypoints.append(q_rand[np.random.randint(0, num_samples - 1), :])\n", 90 | " t = time.time()\n", 91 | " try:\n", 92 | " motion.JointTrajectory(waypoints, timeout=1)\n", 93 | " except:\n", 94 | " continue\n", 95 | " joint_motion.append(time.time() - t)\n", 96 | "\n", 97 | "cartesian_motion = []\n", 98 | "for i in range(num_samples):\n", 99 | " waypoints = []\n", 100 | " for k in range(num_waypoints):\n", 101 | " waypoints.append(T_rand[np.random.randint(0, num_samples - 1)])\n", 102 | " t = time.time()\n", 103 | " try:\n", 104 | " motion.CartesianTrajectory(waypoints, timeout=1)\n", 105 | " except:\n", 106 | " continue\n", 107 | " cartesian_motion.append(time.time() - t)\n" 108 | ] 109 | }, 110 | { 111 | "cell_type": "markdown", 112 | "metadata": {}, 113 | "source": [ 114 | "Pretty-print the resulting average runtimes." 115 | ] 116 | }, 117 | { 118 | "cell_type": "code", 119 | "execution_count": 24, 120 | "metadata": {}, 121 | "outputs": [ 122 | { 123 | "name": "stdout", 124 | "output_type": "stream", 125 | "text": [ 126 | "┌────────────────┬──────────────────┐\n", 127 | "│ Function │ Avg. Runtime (s) │\n", 128 | "├────────────────┼──────────────────┤\n", 129 | "│ fk\u001b[0m │ 3.62e-06\u001b[0m │\n", 130 | "│ ik\u001b[0m │ 3.07e-06\u001b[0m │\n", 131 | "│ JointMotion\u001b[0m │ 1.83e-02\u001b[0m │\n", 132 | "│CartesianMotion\u001b[0m │ 9.66e-03\u001b[0m │\n", 133 | "└────────────────┴──────────────────┘\n", 134 | "\n" 135 | ] 136 | } 137 | ], 138 | "source": [ 139 | "def format_runtime(runtime):\n", 140 | " return format(np.average(runtime), '.2e')\n", 141 | "\n", 142 | "table = ansitable.ANSITable('Function', 'Avg. Runtime (s)', border='thin')\n", 143 | "table.row('fk', format_runtime(fk))\n", 144 | "table.row('ik', format_runtime(ik))\n", 145 | "table.row('JointMotion', format_runtime(joint_motion))\n", 146 | "table.row('CartesianMotion', format_runtime(cartesian_motion))\n", 147 | "table.print()" 148 | ] 149 | } 150 | ], 151 | "metadata": { 152 | "kernelspec": { 153 | "display_name": "Python 3", 154 | "language": "python", 155 | "name": "python3" 156 | }, 157 | "language_info": { 158 | "codemirror_mode": { 159 | "name": "ipython", 160 | "version": 3 161 | }, 162 | "file_extension": ".py", 163 | "mimetype": "text/x-python", 164 | "name": "python", 165 | "nbconvert_exporter": "python", 166 | "pygments_lexer": "ipython3", 167 | "version": "3.10.6" 168 | }, 169 | "orig_nbformat": 4 170 | }, 171 | "nbformat": 4, 172 | "nbformat_minor": 2 173 | } 174 | -------------------------------------------------------------------------------- /examples/pilot_buttons.py: -------------------------------------------------------------------------------- 1 | """ 2 | This example connects to the Desk to listen for pilot button events. 3 | """ 4 | import sys 5 | import time 6 | import panda_py 7 | 8 | 9 | def on_event(event): 10 | """ 11 | Prints the received event. The event is a dictionary 12 | where the triggering button is the key. The value is either 13 | True or False for down and up events respectively. 14 | Refer to the `panda_py.Desk.listen` documentation for mor details. 15 | """ 16 | print(event) 17 | 18 | 19 | if __name__ == '__main__': 20 | if len(sys.argv) < 4: 21 | raise RuntimeError( 22 | f'Usage: python {sys.argv[0]} ' 23 | ) 24 | # Connect to the Desk 25 | d = panda_py.Desk(sys.argv[1], sys.argv[2], sys.argv[3]) 26 | # Listen for 30 seconds 27 | d.listen(on_event) 28 | time.sleep(30) 29 | # Stop listening 30 | d.stop_listen() 31 | -------------------------------------------------------------------------------- /examples/teaching.py: -------------------------------------------------------------------------------- 1 | """ 2 | Simple teaching demonstration. Teaches three joint positions 3 | and connects them using motion generators. Also teaches a joint 4 | trajectory that is then replayed. 5 | """ 6 | import sys 7 | import time 8 | 9 | import panda_py 10 | from panda_py import controllers 11 | 12 | if __name__ == '__main__': 13 | if len(sys.argv) < 2: 14 | raise RuntimeError(f'Usage: python {sys.argv[0]} ') 15 | 16 | panda = panda_py.Panda(sys.argv[1]) 17 | 18 | print('Please teach three poses to the robot.') 19 | positions = [] 20 | panda.teaching_mode(True) 21 | for i in range(3): 22 | print(f'Move the robot into pose {i+1} and press enter to continue.') 23 | input() 24 | positions.append(panda.q) 25 | 26 | panda.teaching_mode(False) 27 | input('Press enter to move through the three poses.') 28 | panda.move_to_joint_position(positions) 29 | 30 | LEN = 10 31 | input( 32 | f'Next, teach a trajectory for {LEN} seconds. Press enter to begin.') 33 | panda.teaching_mode(True) 34 | panda.enable_logging(LEN * 1000) 35 | time.sleep(LEN) 36 | panda.teaching_mode(False) 37 | 38 | q = panda.get_log()['q'] 39 | dq = panda.get_log()['dq'] 40 | 41 | input('Press enter to replay trajectory') 42 | panda.move_to_joint_position(q[0]) 43 | i = 0 44 | ctrl = controllers.JointPosition() 45 | panda.start_controller(ctrl) 46 | with panda.create_context(frequency=1000, max_runtime=LEN) as ctx: 47 | while ctx.ok(): 48 | ctrl.set_control(q[i], dq[i]) 49 | i += 1 50 | -------------------------------------------------------------------------------- /examples/vacuum_object.py: -------------------------------------------------------------------------------- 1 | """ 2 | Uses a vacuum gripper to pick up an object, then drops it. 3 | """ 4 | import sys 5 | import time 6 | from datetime import timedelta 7 | 8 | import panda_py 9 | import panda_py.libfranka 10 | 11 | if __name__ == '__main__': 12 | if len(sys.argv) < 2: 13 | raise RuntimeError(f'Usage: python {sys.argv[0]} ') 14 | 15 | # Connect to the gripper using the same IP as the robot arm 16 | # This doesn't prevent you from connecting to the arm 17 | gripper = panda_py.libfranka.VacuumGripper(sys.argv[1]) 18 | 19 | try: 20 | # Print gripper state. 21 | state = gripper.read_once() 22 | print(f"""Gripper State: 23 | is vacuum within setpoint: {state.in_control_range} 24 | part detached: {state.part_detached} 25 | part present: {state.part_present} 26 | device status: {state.device_status} 27 | actual power: {state.actual_power} 28 | vacuum: {state.vacuum}""") 29 | 30 | print("Vacuuming object") 31 | # The first argument is the vacuum pressure level 32 | # The second argument is how long to try vacuuming for before giving an error 33 | try: 34 | gripper.vacuum(3, timedelta(seconds=1)) 35 | print("Grabbed object.") 36 | except: 37 | # The finally block at the end stops the vacuuming with gripper.stop() 38 | # otherwise it keeps going 39 | raise RuntimeError("Failed to grab object.") 40 | 41 | time.sleep(3) 42 | # Check if the object is still grasped 43 | # This works by checking if the pressure level of the vacuum 44 | # is within a specified range 45 | state = gripper.read_once() 46 | if not state.in_control_range: 47 | raise RuntimeError("Object lost.") 48 | 49 | print("Releasing object.") 50 | # The time argument specifies when to time-out. 51 | try: 52 | gripper.drop_off(timedelta(seconds=1)) 53 | except: 54 | raise RuntimeError("Failed to drop object off.") 55 | finally: 56 | # Stop whatever the gripper is doing when program terminates. 57 | gripper.stop() 58 | -------------------------------------------------------------------------------- /include/constants.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | using Vector7d = Eigen::Matrix; 5 | 6 | const double kTauJMaxData[7] = {87, 87, 87, 87, 12, 12, 12}; 7 | const Vector7d kTauJMax(kTauJMaxData); 8 | 9 | const double kDTauJMaxData[7] = {1000, 1000, 1000, 1000, 1000, 1000, 1000}; 10 | const Vector7d kDTauJMax(kDTauJMaxData); 11 | 12 | const double kJointPositionStartData[7] = {0.0, -M_PI_4, 0.0, -3 * M_PI_4, 13 | 0.0, M_PI_2, M_PI_4}; 14 | const Vector7d kJointPositionStart(kJointPositionStartData); 15 | 16 | const double kLowerJointLimitsData[7] = {-2.8973, -1.7628, -2.8973, -3.0718, 17 | -2.8973, -0.0175, -2.8973}; 18 | const Vector7d kLowerJointLimits(kLowerJointLimitsData); 19 | 20 | const double kUpperJointLimitsData[7] = {2.8973, 1.7628, 2.8973, -0.0698, 21 | 2.8973, 3.7525, 2.8973}; 22 | const Vector7d kUpperJointLimits(kUpperJointLimitsData); 23 | 24 | const double kQMaxVelocityData[7] = {2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100}; //{2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100}; 25 | const Vector7d kQMaxVelocity(kQMaxVelocityData); 26 | 27 | const double kQMaxAccelerationData[7] = {15, 7.5, 10, 12.5, 15, 20, 20}; 28 | const Vector7d kQMaxAcceleration(kQMaxAccelerationData); 29 | 30 | const double kXMaxVelocityData[4] = {1.7, 1.7, 1.7, 2.5}; //{1.7, 1.7, 1.7, 2.5}; 31 | const Eigen::Vector4d kXMaxVelocity(kXMaxVelocityData); 32 | 33 | const double kXMaxAccelerationData[4] = {13, 13, 13, 25}; 34 | const Eigen::Vector4d kXMaxAcceleration(kXMaxAccelerationData); 35 | 36 | const double kPDZoneWidthData[7] = {0.12, 0.09, 0.09, 0.09, 37 | 0.0349, 0.0349, 0.0349}; 38 | const Vector7d kPDZoneWidth(kPDZoneWidthData); 39 | 40 | const double kDZoneWidthData[7] = {0.12, 0.09, 0.09, 0.09, 41 | 0.0349, 0.0349, 0.0349}; 42 | const Vector7d kDZoneWidth(kDZoneWidthData); 43 | 44 | const double kPDZoneStiffnessData[7] = {2000.0, 2000.0, 1000.0, 1000.0, 45 | 500.0, 200.0, 200.0}; 46 | const Vector7d kPDZoneStiffness(kPDZoneStiffnessData); 47 | 48 | const double kPDZoneDampingData[7] = {30.0, 30.0, 30.0, 10.0, 5.0, 5.0, 5.0}; 49 | const Vector7d kPDZoneDamping(kPDZoneDamping); 50 | 51 | const double kDZoneDampingData[7] = {30.0, 30.0, 30.0, 10.0, 5.0, 5.0, 5.0}; 52 | const Vector7d kDZoneDamping(kDZoneDampingData); 53 | -------------------------------------------------------------------------------- /include/controllers/applied_force.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | #include "controllers/controller.h" 6 | #include "utils.h" 7 | 8 | class AppliedForce : public TorqueController { 9 | public: 10 | static const double kDefaultFilterCoeff; 11 | static const Vector7d kDefaultDamping; 12 | 13 | AppliedForce(const Vector7d &damping = kDefaultDamping, 14 | const double filter_coeff = kDefaultFilterCoeff); 15 | 16 | franka::Torques step(const franka::RobotState &robot_state, 17 | franka::Duration &duration) override; 18 | void setControl(const Vector6d &force); 19 | void setDamping(const Vector7d &damping); 20 | void setFilter(const double filter_coeff); 21 | void start(const franka::RobotState &robot_state, 22 | std::shared_ptr model) override; 23 | void stop(const franka::RobotState &robot_state, 24 | std::shared_ptr model) override; 25 | bool isRunning() override; 26 | const std::string name() override; 27 | 28 | private: 29 | Vector6d f_d_, f_d_target_; 30 | Vector7d K_d_, K_d_target_; 31 | double filter_coeff_; 32 | std::mutex mux_; 33 | std::atomic motion_finished_; 34 | std::shared_ptr model_; 35 | 36 | void _updateFilter(); 37 | }; 38 | -------------------------------------------------------------------------------- /include/controllers/applied_torque.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | #include "controllers/controller.h" 6 | #include "utils.h" 7 | 8 | class AppliedTorque : public TorqueController { 9 | public: 10 | static const double kDefaultFilterCoeff; 11 | static const Vector7d kDefaultDamping; 12 | 13 | AppliedTorque(const Vector7d &damping = kDefaultDamping, 14 | const double filter_coeff = kDefaultFilterCoeff); 15 | 16 | franka::Torques step(const franka::RobotState &robot_state, 17 | franka::Duration &duration) override; 18 | void setControl(const Vector7d &torque); 19 | void setDamping(const Vector7d &damping); 20 | void setFilter(const double filter_coeff); 21 | void start(const franka::RobotState &robot_state, 22 | std::shared_ptr model) override; 23 | void stop(const franka::RobotState &robot_state, 24 | std::shared_ptr model) override; 25 | bool isRunning() override; 26 | const std::string name() override; 27 | 28 | private: 29 | Vector7d tau_d_, tau_d_target_, K_d_, K_d_target_; 30 | double filter_coeff_; 31 | std::mutex mux_; 32 | std::atomic motion_finished_; 33 | 34 | void _updateFilter(); 35 | }; 36 | -------------------------------------------------------------------------------- /include/controllers/cartesian_impedance.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | #include "constants.h" 6 | #include "controllers/controller.h" 7 | #include "utils.h" 8 | 9 | class CartesianImpedance : public TorqueController { 10 | public: 11 | static const Eigen::Matrix kDefaultImpedance; 12 | static const double kDefaultDampingRatio; 13 | static const double kDefaultNullspaceStiffness; 14 | static const double kDefaultFilterCoeff; 15 | 16 | CartesianImpedance(const Eigen::Matrix &impedance = 17 | kDefaultImpedance, 18 | const double &damping_ratio = kDefaultDampingRatio, 19 | const double &nullspace_stiffness = 20 | kDefaultNullspaceStiffness, 21 | const double &filter_coeff = kDefaultFilterCoeff); 22 | 23 | franka::Torques step(const franka::RobotState &robot_state, 24 | franka::Duration &duration) override; 25 | void setControl(const Eigen::Vector3d &position, 26 | const Eigen::Vector4d &orientation, 27 | const Vector7d &q_nullspace = kJointPositionStart); 28 | void setImpedance(const Eigen::Matrix &impedance); 29 | void setDampingRatio(const double &damping_ratio); 30 | void setNullspaceStiffness(const double &nullspace_stiffness); 31 | void setFilter(const double filter_coeff); 32 | void start(const franka::RobotState &robot_state, 33 | std::shared_ptr model) override; 34 | void stop(const franka::RobotState &robot_state, 35 | std::shared_ptr model) override; 36 | bool isRunning() override; 37 | const std::string name() override; 38 | 39 | private: 40 | Eigen::Matrix K_p_, K_d_, K_p_target_, K_d_target_; 41 | Eigen::Vector3d position_d_, position_d_target_; 42 | Eigen::Quaterniond orientation_d_, orientation_d_target_; 43 | Vector7d q_nullspace_d_, q_nullspace_d_target_; 44 | double filter_coeff_, nullspace_stiffness_, nullspace_stiffnes_target_, 45 | damping_ratio_; 46 | std::mutex mux_; 47 | std::atomic motion_finished_; 48 | std::shared_ptr model_; 49 | 50 | void _updateFilter(); 51 | void _computeDamping(); 52 | }; 53 | -------------------------------------------------------------------------------- /include/controllers/cartesian_trajectory.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "controllers/cartesian_impedance.h" 3 | #include "motion/generators.h" 4 | 5 | namespace controllers { 6 | 7 | class CartesianTrajectory : public CartesianImpedance { 8 | public: 9 | static const double kDefaultDqThreshold; 10 | static const double kDefaultNullspaceStiffness; 11 | static const Eigen::Matrix kDefaultImpedance; 12 | 13 | CartesianTrajectory(std::shared_ptr trajectory, 14 | const Vector7d &q_init, 15 | const Eigen::Matrix &impedance = kDefaultImpedance, 16 | const double &damping_ratio = kDefaultDampingRatio, 17 | const double &nullspace_stiffness = kDefaultNullspaceStiffness, 18 | const double dq_threshold = kDefaultDqThreshold, 19 | const double filter_coeff = kDefaultFilterCoeff); 20 | 21 | franka::Torques step(const franka::RobotState &robot_state, 22 | franka::Duration &duration) override; 23 | 24 | const std::string name() override; 25 | 26 | private: 27 | std::shared_ptr traj_; 28 | Vector7d q_init_; 29 | double dq_threshold_; 30 | }; 31 | 32 | } // namespace 33 | -------------------------------------------------------------------------------- /include/controllers/controller.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #include 5 | 6 | class TorqueController { 7 | public: 8 | virtual franka::Torques step(const franka::RobotState &robot_state, 9 | franka::Duration &duration) = 0; 10 | virtual void start(const franka::RobotState &robot_state, 11 | std::shared_ptr model) = 0; 12 | virtual void stop(const franka::RobotState &robot_state, 13 | std::shared_ptr model) = 0; 14 | virtual bool isRunning() = 0; 15 | virtual const std::string name() = 0; 16 | 17 | void setTime(double time) { time_ = time; } 18 | 19 | double getTime() { return time_; } 20 | 21 | private: 22 | std::atomic time_; 23 | }; 24 | -------------------------------------------------------------------------------- /include/controllers/force.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | #include "controllers/controller.h" 6 | #include "utils.h" 7 | 8 | class Force : public TorqueController { 9 | public: 10 | static const double kDefaultFilterCoeff; 11 | static const double kDefaultProportionalGain; 12 | static const double kDefaultIntegralGain; 13 | static const double kDefaultThreshold; 14 | static const Vector7d kDefaultDamping; 15 | 16 | Force(const double &k_p = kDefaultProportionalGain, 17 | const double &k_i = kDefaultIntegralGain, 18 | const Vector7d &damping = kDefaultDamping, 19 | const double &threshold = kDefaultThreshold, 20 | const double &filter_coeff = kDefaultFilterCoeff); 21 | 22 | franka::Torques step(const franka::RobotState &robot_state, 23 | franka::Duration &duration) override; 24 | void setControl(const Eigen::Vector3d &force); 25 | void setProportionalGain(const double &k_p); 26 | void setIntegralGain(const double &k_i); 27 | void setThreshold(const double &threshold); 28 | void setDamping(const Vector7d &damping); 29 | void setFilter(const double &filter_coeff); 30 | void start(const franka::RobotState &robot_state, 31 | std::shared_ptr model) override; 32 | void stop(const franka::RobotState &robot_state, 33 | std::shared_ptr model) override; 34 | bool isRunning() override; 35 | const std::string name() override; 36 | 37 | private: 38 | Vector7d tau_ext_init_, tau_error_integral_, K_d_, K_d_target_; 39 | Eigen::Vector3d f_d_, f_d_target_, position_init_; 40 | double filter_coeff_, k_p_, k_i_, k_p_target_, k_i_target_, threshold_, 41 | threshold_target_; 42 | std::mutex mux_; 43 | std::atomic motion_finished_; 44 | std::shared_ptr model_; 45 | 46 | void _updateFilter(); 47 | }; 48 | -------------------------------------------------------------------------------- /include/controllers/integrated_velocity.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | #include "controllers/controller.h" 6 | #include "utils.h" 7 | 8 | class IntegratedVelocity : public TorqueController { 9 | public: 10 | static const Vector7d kDefaultStiffness; 11 | static const Vector7d kDefaultDamping; 12 | 13 | IntegratedVelocity(const Vector7d &stiffness = kDefaultStiffness, 14 | const Vector7d &damping = kDefaultDamping); 15 | 16 | franka::Torques step(const franka::RobotState &robot_state, 17 | franka::Duration &duration) override; 18 | 19 | Vector7d getQd(); 20 | void setControl(const Vector7d &velocity); 21 | void setStiffness(const Vector7d &stiffness); 22 | void setDamping(const Vector7d &damping); 23 | void start(const franka::RobotState &robot_state, std::shared_ptr model) override; 24 | void stop(const franka::RobotState &robot_state, std::shared_ptr model) override; 25 | bool isRunning() override; 26 | const std::string name() override; 27 | 28 | private: 29 | Vector7d K_p_, K_d_, q_d_, dq_d_; 30 | std::mutex mux_; 31 | std::atomic motion_finished_; 32 | }; 33 | -------------------------------------------------------------------------------- /include/controllers/joint_limits/virtual_wall.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace controllers { 4 | namespace joint_limits { 5 | 6 | class VirtualWall { 7 | public: 8 | VirtualWall() = delete; 9 | 10 | VirtualWall(const double& soft_upper_joint_position_limit, 11 | const double& soft_lower_joint_position_limit, 12 | const double& PD_zone_width, const double& D_zone_width, 13 | const double& PD_zone_stiffness, const double& PD_zone_damping, 14 | const double& D_zone_damping); 15 | 16 | double computeTorque(const double& q, const double& dq); 17 | void reset(); 18 | 19 | private: 20 | enum class MotionInWall { 21 | EnteringNormal, 22 | PenetratingLowerLimit, 23 | LeavingLowerLimit, 24 | PenetratingUpperLimit, 25 | LeavingUpperLimit 26 | }; 27 | double soft_upper_joint_position_limit_{0}; 28 | double soft_lower_joint_position_limit_{0}; 29 | double PD_zone_width_{0}; 30 | double D_zone_width_{0}; 31 | double PD_zone_stiffness_{0}; 32 | double PD_zone_damping_{0}; 33 | double D_zone_damping_{0}; 34 | bool initialized_{false}; 35 | bool moving_wall_{false}; 36 | double zone_width_scale_{1}; 37 | 38 | static bool inRange(double low, double high, double x); 39 | static double positiveCheck(double value); 40 | void init(const double& q, const double& dq); 41 | void adjustMovingWall(const double& q, const double& dq); 42 | MotionInWall getMotionInWall(const double& q, const double& dq) const; 43 | }; 44 | } // namespace joint_limits 45 | } // namespace controllers 46 | -------------------------------------------------------------------------------- /include/controllers/joint_limits/virtual_wall_controller.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "utils.h" 8 | #include "controllers/joint_limits/virtual_wall.h" 9 | 10 | namespace controllers { 11 | namespace joint_limits { 12 | 13 | class VirtualWallController { 14 | public: 15 | VirtualWallController( 16 | const Vector7d& soft_upper_joint_position_limits, 17 | const Vector7d& soft_lower_joint_position_limits, 18 | const Vector7d& PD_zone_widths, 19 | const Vector7d& D_zone_widths, 20 | const Vector7d& PD_zone_stiffnesses, 21 | const Vector7d& PD_zone_dampings, 22 | const Vector7d& D_zone_dampings) { 23 | for (size_t i = 0; i < 7; i++) { 24 | virtual_walls_.at(i) = std::make_unique( 25 | soft_upper_joint_position_limits[i], 26 | soft_lower_joint_position_limits[i], PD_zone_widths[i], 27 | D_zone_widths[i], PD_zone_stiffnesses[i], PD_zone_dampings[i], 28 | D_zone_dampings[i]); 29 | } 30 | } 31 | 32 | VirtualWallController() = delete; 33 | 34 | void computeTorque(const Array7d& q, 35 | const Array7d& dq, 36 | Array7d& torque) { 37 | for (size_t i = 0; i < 7; i++) { 38 | torque[i] = virtual_walls_[i]->computeTorque(q[i], dq[i]); 39 | }; 40 | } 41 | 42 | void reset() { 43 | for (auto& jw : virtual_walls_) { 44 | jw->reset(); 45 | } 46 | } 47 | 48 | private: 49 | std::array, 7> virtual_walls_; 50 | }; 51 | } // namespace joint_limits 52 | } // namespace controllers 53 | -------------------------------------------------------------------------------- /include/controllers/joint_position.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | #include "controllers/controller.h" 6 | #include "utils.h" 7 | 8 | class JointPosition : public TorqueController { 9 | public: 10 | static const Vector7d kDefaultStiffness; 11 | static const Vector7d kDefaultDamping; 12 | static const Vector7d kDefaultDqd; 13 | static const double kDefaultFilterCoeff; 14 | 15 | JointPosition(const Vector7d &stiffness = kDefaultStiffness, 16 | const Vector7d &damping = kDefaultDamping, 17 | const double filter_coeff = kDefaultFilterCoeff); 18 | 19 | franka::Torques step(const franka::RobotState &robot_state, 20 | franka::Duration &duration) override; 21 | void setControl(const Vector7d &position, const Vector7d &velocity = kDefaultDqd); 22 | void setStiffness(const Vector7d &stiffness); 23 | void setDamping(const Vector7d &damping); 24 | void setFilter(const double filter_coeff); 25 | void start(const franka::RobotState &robot_state, std::shared_ptr model) override; 26 | void stop(const franka::RobotState &robot_state, std::shared_ptr model) override; 27 | bool isRunning() override; 28 | const std::string name() override; 29 | 30 | private: 31 | Vector7d K_p_, K_d_, q_d_, dq_d_, K_p_target_, K_d_target_, q_d_target_, dq_d_target_; 32 | double filter_coeff_; 33 | std::mutex mux_; 34 | std::atomic motion_finished_; 35 | 36 | void _updateFilter(); 37 | }; 38 | -------------------------------------------------------------------------------- /include/controllers/joint_trajectory.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "controllers/joint_position.h" 3 | #include "motion/generators.h" 4 | 5 | namespace controllers { 6 | 7 | class JointTrajectory : public JointPosition { 8 | public: 9 | static const double kDefaultDqThreshold; 10 | 11 | JointTrajectory(std::shared_ptr trajectory, 12 | const Vector7d &stiffness = kDefaultStiffness, 13 | const Vector7d &damping = kDefaultDamping, 14 | const double dq_threshold = kDefaultDqThreshold, 15 | const double filter_coeff = kDefaultFilterCoeff); 16 | 17 | franka::Torques step(const franka::RobotState &robot_state, 18 | franka::Duration &duration) override; 19 | 20 | const std::string name() override; 21 | 22 | private: 23 | std::shared_ptr traj_; 24 | double dq_threshold_; 25 | }; 26 | 27 | } // namespace 28 | -------------------------------------------------------------------------------- /include/kinematics/fk.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Eigen/Dense" 4 | 5 | namespace kinematics { 6 | 7 | Eigen::Matrix fk(const Eigen::Matrix &q) { 8 | Eigen::Matrix pose; 9 | pose.setZero(); 10 | pose.row(0)(0) = 11 | 1.0 * std::sin(q[0]) * std::sin(q[2]) * std::sin(q[3]) * std::sin(q[5]) * 12 | std::sin(q[6] + M_PI_4) + 13 | 1.0 * std::sin(q[0]) * std::sin(q[2]) * std::sin(q[4]) * std::cos(q[3]) * 14 | std::cos(q[6] + M_PI_4) - 15 | 1.0 * std::sin(q[0]) * std::sin(q[2]) * std::sin(q[6] + M_PI_4) * 16 | std::cos(q[3]) * std::cos(q[4]) * std::cos(q[5]) - 17 | 1.0 * std::sin(q[0]) * std::sin(q[4]) * std::sin(q[6] + M_PI_4) * 18 | std::cos(q[2]) * std::cos(q[5]) - 19 | 1.0 * std::sin(q[0]) * std::cos(q[2]) * std::cos(q[4]) * 20 | std::cos(q[6] + M_PI_4) - 21 | 1.0 * std::sin(q[1]) * std::sin(q[3]) * std::sin(q[4]) * std::cos(q[0]) * 22 | std::cos(q[6] + M_PI_4) + 23 | 1.0 * std::sin(q[1]) * std::sin(q[3]) * std::sin(q[6] + M_PI_4) * 24 | std::cos(q[0]) * std::cos(q[4]) * std::cos(q[5]) + 25 | 1.0 * std::sin(q[1]) * std::sin(q[5]) * std::sin(q[6] + M_PI_4) * 26 | std::cos(q[0]) * std::cos(q[3]) - 27 | 1.0 * std::sin(q[2]) * std::sin(q[4]) * std::sin(q[6] + M_PI_4) * 28 | std::cos(q[0]) * std::cos(q[1]) * std::cos(q[5]) - 29 | 1.0 * std::sin(q[2]) * std::cos(q[0]) * std::cos(q[1]) * std::cos(q[4]) * 30 | std::cos(q[6] + M_PI_4) - 31 | 1.0 * std::sin(q[3]) * std::sin(q[5]) * std::sin(q[6] + M_PI_4) * 32 | std::cos(q[0]) * std::cos(q[1]) * std::cos(q[2]) - 33 | 1.0 * std::sin(q[4]) * std::cos(q[0]) * std::cos(q[1]) * std::cos(q[2]) * 34 | std::cos(q[3]) * std::cos(q[6] + M_PI_4) + 35 | 1.0 * std::sin(q[6] + M_PI_4) * std::cos(q[0]) * std::cos(q[1]) * 36 | std::cos(q[2]) * std::cos(q[3]) * std::cos(q[4]) * std::cos(q[5]); 37 | pose.row(0)(1) = 38 | 1.0 * std::sin(q[0]) * std::sin(q[2]) * std::sin(q[3]) * std::sin(q[5]) * 39 | std::cos(q[6] + M_PI_4) - 40 | 1.0 * std::sin(q[0]) * std::sin(q[2]) * std::sin(q[4]) * 41 | std::sin(q[6] + M_PI_4) * std::cos(q[3]) - 42 | 1.0 * std::sin(q[0]) * std::sin(q[2]) * std::cos(q[3]) * std::cos(q[4]) * 43 | std::cos(q[5]) * std::cos(q[6] + M_PI_4) - 44 | 1.0 * std::sin(q[0]) * std::sin(q[4]) * std::cos(q[2]) * std::cos(q[5]) * 45 | std::cos(q[6] + M_PI_4) + 46 | 1.0 * std::sin(q[0]) * std::sin(q[6] + M_PI_4) * std::cos(q[2]) * 47 | std::cos(q[4]) + 48 | 1.0 * std::sin(q[1]) * std::sin(q[3]) * std::sin(q[4]) * 49 | std::sin(q[6] + M_PI_4) * std::cos(q[0]) + 50 | 1.0 * std::sin(q[1]) * std::sin(q[3]) * std::cos(q[0]) * std::cos(q[4]) * 51 | std::cos(q[5]) * std::cos(q[6] + M_PI_4) + 52 | 1.0 * std::sin(q[1]) * std::sin(q[5]) * std::cos(q[0]) * std::cos(q[3]) * 53 | std::cos(q[6] + M_PI_4) - 54 | 1.0 * std::sin(q[2]) * std::sin(q[4]) * std::cos(q[0]) * std::cos(q[1]) * 55 | std::cos(q[5]) * std::cos(q[6] + M_PI_4) + 56 | 1.0 * std::sin(q[2]) * std::sin(q[6] + M_PI_4) * std::cos(q[0]) * 57 | std::cos(q[1]) * std::cos(q[4]) - 58 | 1.0 * std::sin(q[3]) * std::sin(q[5]) * std::cos(q[0]) * std::cos(q[1]) * 59 | std::cos(q[2]) * std::cos(q[6] + M_PI_4) + 60 | 1.0 * std::sin(q[4]) * std::sin(q[6] + M_PI_4) * std::cos(q[0]) * 61 | std::cos(q[1]) * std::cos(q[2]) * std::cos(q[3]) + 62 | 1.0 * std::cos(q[0]) * std::cos(q[1]) * std::cos(q[2]) * std::cos(q[3]) * 63 | std::cos(q[4]) * std::cos(q[5]) * std::cos(q[6] + M_PI_4); 64 | pose.row(0)(2) = -1.0 * 65 | (((std::sin(q[0]) * std::sin(q[2]) - 66 | std::cos(q[0]) * std::cos(q[1]) * std::cos(q[2])) * 67 | std::cos(q[3]) - 68 | std::sin(q[1]) * std::sin(q[3]) * std::cos(q[0])) * 69 | std::cos(q[4]) + 70 | (std::sin(q[0]) * std::cos(q[2]) + 71 | std::sin(q[2]) * std::cos(q[0]) * std::cos(q[1])) * 72 | std::sin(q[4])) * 73 | std::sin(q[5]) - 74 | 1.0 * 75 | ((std::sin(q[0]) * std::sin(q[2]) - 76 | std::cos(q[0]) * std::cos(q[1]) * std::cos(q[2])) * 77 | std::sin(q[3]) + 78 | std::sin(q[1]) * std::cos(q[0]) * std::cos(q[3])) * 79 | std::cos(q[5]); 80 | pose.row(0)(3) = 81 | -0.21000000000000002 * 82 | (((std::sin(q[0]) * std::sin(q[2]) - 83 | std::cos(q[0]) * std::cos(q[1]) * std::cos(q[2])) * 84 | std::cos(q[3]) - 85 | std::sin(q[1]) * std::sin(q[3]) * std::cos(q[0])) * 86 | std::cos(q[4]) + 87 | (std::sin(q[0]) * std::cos(q[2]) + 88 | std::sin(q[2]) * std::cos(q[0]) * std::cos(q[1])) * 89 | std::sin(q[4])) * 90 | std::sin(q[5]) - 91 | 0.087999999999999995 * 92 | (((std::sin(q[0]) * std::sin(q[2]) - 93 | std::cos(q[0]) * std::cos(q[1]) * std::cos(q[2])) * 94 | std::cos(q[3]) - 95 | std::sin(q[1]) * std::sin(q[3]) * std::cos(q[0])) * 96 | std::cos(q[4]) + 97 | (std::sin(q[0]) * std::cos(q[2]) + 98 | std::sin(q[2]) * std::cos(q[0]) * std::cos(q[1])) * 99 | std::sin(q[4])) * 100 | std::cos(q[5]) + 101 | 0.087999999999999995 * 102 | ((std::sin(q[0]) * std::sin(q[2]) - 103 | std::cos(q[0]) * std::cos(q[1]) * std::cos(q[2])) * 104 | std::sin(q[3]) + 105 | std::sin(q[1]) * std::cos(q[0]) * std::cos(q[3])) * 106 | std::sin(q[5]) - 107 | 0.21000000000000002 * 108 | ((std::sin(q[0]) * std::sin(q[2]) - 109 | std::cos(q[0]) * std::cos(q[1]) * std::cos(q[2])) * 110 | std::sin(q[3]) + 111 | std::sin(q[1]) * std::cos(q[0]) * std::cos(q[3])) * 112 | std::cos(q[5]) + 113 | 0.38400000000000001 * 114 | (std::sin(q[0]) * std::sin(q[2]) - 115 | std::cos(q[0]) * std::cos(q[1]) * std::cos(q[2])) * 116 | std::sin(q[3]) + 117 | 0.082500000000000004 * 118 | (std::sin(q[0]) * std::sin(q[2]) - 119 | std::cos(q[0]) * std::cos(q[1]) * std::cos(q[2])) * 120 | std::cos(q[3]) - 121 | 0.082500000000000004 * std::sin(q[0]) * std::sin(q[2]) - 122 | 0.082500000000000004 * std::sin(q[1]) * std::sin(q[3]) * std::cos(q[0]) + 123 | 0.38400000000000001 * std::sin(q[1]) * std::cos(q[0]) * std::cos(q[3]) + 124 | 0.316 * std::sin(q[1]) * std::cos(q[0]) + 125 | 0.082500000000000004 * std::cos(q[0]) * std::cos(q[1]) * std::cos(q[2]); 126 | pose.row(1)(0) = 127 | -1.0 * std::sin(q[0]) * std::sin(q[1]) * std::sin(q[3]) * std::sin(q[4]) * 128 | std::cos(q[6] + M_PI_4) + 129 | 1.0 * std::sin(q[0]) * std::sin(q[1]) * std::sin(q[3]) * 130 | std::sin(q[6] + M_PI_4) * std::cos(q[4]) * std::cos(q[5]) + 131 | 1.0 * std::sin(q[0]) * std::sin(q[1]) * std::sin(q[5]) * 132 | std::sin(q[6] + M_PI_4) * std::cos(q[3]) - 133 | 1.0 * std::sin(q[0]) * std::sin(q[2]) * std::sin(q[4]) * 134 | std::sin(q[6] + M_PI_4) * std::cos(q[1]) * std::cos(q[5]) - 135 | 1.0 * std::sin(q[0]) * std::sin(q[2]) * std::cos(q[1]) * std::cos(q[4]) * 136 | std::cos(q[6] + M_PI_4) - 137 | 1.0 * std::sin(q[0]) * std::sin(q[3]) * std::sin(q[5]) * 138 | std::sin(q[6] + M_PI_4) * std::cos(q[1]) * std::cos(q[2]) - 139 | 1.0 * std::sin(q[0]) * std::sin(q[4]) * std::cos(q[1]) * std::cos(q[2]) * 140 | std::cos(q[3]) * std::cos(q[6] + M_PI_4) + 141 | 1.0 * std::sin(q[0]) * std::sin(q[6] + M_PI_4) * std::cos(q[1]) * 142 | std::cos(q[2]) * std::cos(q[3]) * std::cos(q[4]) * std::cos(q[5]) - 143 | 1.0 * std::sin(q[2]) * std::sin(q[3]) * std::sin(q[5]) * 144 | std::sin(q[6] + M_PI_4) * std::cos(q[0]) - 145 | 1.0 * std::sin(q[2]) * std::sin(q[4]) * std::cos(q[0]) * std::cos(q[3]) * 146 | std::cos(q[6] + M_PI_4) + 147 | 1.0 * std::sin(q[2]) * std::sin(q[6] + M_PI_4) * std::cos(q[0]) * 148 | std::cos(q[3]) * std::cos(q[4]) * std::cos(q[5]) + 149 | 1.0 * std::sin(q[4]) * std::sin(q[6] + M_PI_4) * std::cos(q[0]) * 150 | std::cos(q[2]) * std::cos(q[5]) + 151 | 1.0 * std::cos(q[0]) * std::cos(q[2]) * std::cos(q[4]) * 152 | std::cos(q[6] + M_PI_4); 153 | pose.row(1)(1) = 154 | 1.0 * std::sin(q[0]) * std::sin(q[1]) * std::sin(q[3]) * std::sin(q[4]) * 155 | std::sin(q[6] + M_PI_4) + 156 | 1.0 * std::sin(q[0]) * std::sin(q[1]) * std::sin(q[3]) * std::cos(q[4]) * 157 | std::cos(q[5]) * std::cos(q[6] + M_PI_4) + 158 | 1.0 * std::sin(q[0]) * std::sin(q[1]) * std::sin(q[5]) * std::cos(q[3]) * 159 | std::cos(q[6] + M_PI_4) - 160 | 1.0 * std::sin(q[0]) * std::sin(q[2]) * std::sin(q[4]) * std::cos(q[1]) * 161 | std::cos(q[5]) * std::cos(q[6] + M_PI_4) + 162 | 1.0 * std::sin(q[0]) * std::sin(q[2]) * std::sin(q[6] + M_PI_4) * 163 | std::cos(q[1]) * std::cos(q[4]) - 164 | 1.0 * std::sin(q[0]) * std::sin(q[3]) * std::sin(q[5]) * std::cos(q[1]) * 165 | std::cos(q[2]) * std::cos(q[6] + M_PI_4) + 166 | 1.0 * std::sin(q[0]) * std::sin(q[4]) * std::sin(q[6] + M_PI_4) * 167 | std::cos(q[1]) * std::cos(q[2]) * std::cos(q[3]) + 168 | 1.0 * std::sin(q[0]) * std::cos(q[1]) * std::cos(q[2]) * std::cos(q[3]) * 169 | std::cos(q[4]) * std::cos(q[5]) * std::cos(q[6] + M_PI_4) - 170 | 1.0 * std::sin(q[2]) * std::sin(q[3]) * std::sin(q[5]) * std::cos(q[0]) * 171 | std::cos(q[6] + M_PI_4) + 172 | 1.0 * std::sin(q[2]) * std::sin(q[4]) * std::sin(q[6] + M_PI_4) * 173 | std::cos(q[0]) * std::cos(q[3]) + 174 | 1.0 * std::sin(q[2]) * std::cos(q[0]) * std::cos(q[3]) * std::cos(q[4]) * 175 | std::cos(q[5]) * std::cos(q[6] + M_PI_4) + 176 | 1.0 * std::sin(q[4]) * std::cos(q[0]) * std::cos(q[2]) * std::cos(q[5]) * 177 | std::cos(q[6] + M_PI_4) - 178 | 1.0 * std::sin(q[6] + M_PI_4) * std::cos(q[0]) * std::cos(q[2]) * 179 | std::cos(q[4]); 180 | pose.row(1)(2) = 1.0 * 181 | (((std::sin(q[0]) * std::cos(q[1]) * std::cos(q[2]) + 182 | std::sin(q[2]) * std::cos(q[0])) * 183 | std::cos(q[3]) + 184 | std::sin(q[0]) * std::sin(q[1]) * std::sin(q[3])) * 185 | std::cos(q[4]) - 186 | (std::sin(q[0]) * std::sin(q[2]) * std::cos(q[1]) - 187 | std::cos(q[0]) * std::cos(q[2])) * 188 | std::sin(q[4])) * 189 | std::sin(q[5]) + 190 | 1.0 * 191 | ((std::sin(q[0]) * std::cos(q[1]) * std::cos(q[2]) + 192 | std::sin(q[2]) * std::cos(q[0])) * 193 | std::sin(q[3]) - 194 | std::sin(q[0]) * std::sin(q[1]) * std::cos(q[3])) * 195 | std::cos(q[5]); 196 | pose.row(1)(3) = 197 | 0.21000000000000002 * 198 | (((std::sin(q[0]) * std::cos(q[1]) * std::cos(q[2]) + 199 | std::sin(q[2]) * std::cos(q[0])) * 200 | std::cos(q[3]) + 201 | std::sin(q[0]) * std::sin(q[1]) * std::sin(q[3])) * 202 | std::cos(q[4]) - 203 | (std::sin(q[0]) * std::sin(q[2]) * std::cos(q[1]) - 204 | std::cos(q[0]) * std::cos(q[2])) * 205 | std::sin(q[4])) * 206 | std::sin(q[5]) + 207 | 0.087999999999999995 * 208 | (((std::sin(q[0]) * std::cos(q[1]) * std::cos(q[2]) + 209 | std::sin(q[2]) * std::cos(q[0])) * 210 | std::cos(q[3]) + 211 | std::sin(q[0]) * std::sin(q[1]) * std::sin(q[3])) * 212 | std::cos(q[4]) - 213 | (std::sin(q[0]) * std::sin(q[2]) * std::cos(q[1]) - 214 | std::cos(q[0]) * std::cos(q[2])) * 215 | std::sin(q[4])) * 216 | std::cos(q[5]) - 217 | 0.087999999999999995 * 218 | ((std::sin(q[0]) * std::cos(q[1]) * std::cos(q[2]) + 219 | std::sin(q[2]) * std::cos(q[0])) * 220 | std::sin(q[3]) - 221 | std::sin(q[0]) * std::sin(q[1]) * std::cos(q[3])) * 222 | std::sin(q[5]) + 223 | 0.21000000000000002 * 224 | ((std::sin(q[0]) * std::cos(q[1]) * std::cos(q[2]) + 225 | std::sin(q[2]) * std::cos(q[0])) * 226 | std::sin(q[3]) - 227 | std::sin(q[0]) * std::sin(q[1]) * std::cos(q[3])) * 228 | std::cos(q[5]) - 229 | 0.38400000000000001 * 230 | (std::sin(q[0]) * std::cos(q[1]) * std::cos(q[2]) + 231 | std::sin(q[2]) * std::cos(q[0])) * 232 | std::sin(q[3]) - 233 | 0.082500000000000004 * 234 | (std::sin(q[0]) * std::cos(q[1]) * std::cos(q[2]) + 235 | std::sin(q[2]) * std::cos(q[0])) * 236 | std::cos(q[3]) - 237 | 0.082500000000000004 * std::sin(q[0]) * std::sin(q[1]) * std::sin(q[3]) + 238 | 0.38400000000000001 * std::sin(q[0]) * std::sin(q[1]) * std::cos(q[3]) + 239 | 0.316 * std::sin(q[0]) * std::sin(q[1]) + 240 | 0.082500000000000004 * std::sin(q[0]) * std::cos(q[1]) * std::cos(q[2]) + 241 | 0.082500000000000004 * std::sin(q[2]) * std::cos(q[0]); 242 | pose.row(2)(0) = 1.0 * std::sin(q[1]) * std::sin(q[2]) * std::sin(q[4]) * 243 | std::sin(q[6] + M_PI_4) * std::cos(q[5]) + 244 | 1.0 * std::sin(q[1]) * std::sin(q[2]) * std::cos(q[4]) * 245 | std::cos(q[6] + M_PI_4) + 246 | 1.0 * std::sin(q[1]) * std::sin(q[3]) * std::sin(q[5]) * 247 | std::sin(q[6] + M_PI_4) * std::cos(q[2]) + 248 | 1.0 * std::sin(q[1]) * std::sin(q[4]) * std::cos(q[2]) * 249 | std::cos(q[3]) * std::cos(q[6] + M_PI_4) - 250 | 1.0 * std::sin(q[1]) * std::sin(q[6] + M_PI_4) * 251 | std::cos(q[2]) * std::cos(q[3]) * std::cos(q[4]) * 252 | std::cos(q[5]) - 253 | 1.0 * std::sin(q[3]) * std::sin(q[4]) * std::cos(q[1]) * 254 | std::cos(q[6] + M_PI_4) + 255 | 1.0 * std::sin(q[3]) * std::sin(q[6] + M_PI_4) * 256 | std::cos(q[1]) * std::cos(q[4]) * std::cos(q[5]) + 257 | 1.0 * std::sin(q[5]) * std::sin(q[6] + M_PI_4) * 258 | std::cos(q[1]) * std::cos(q[3]); 259 | pose.row(2)(1) = 260 | 1.0 * std::sin(q[1]) * std::sin(q[2]) * std::sin(q[4]) * std::cos(q[5]) * 261 | std::cos(q[6] + M_PI_4) - 262 | 1.0 * std::sin(q[1]) * std::sin(q[2]) * std::sin(q[6] + M_PI_4) * 263 | std::cos(q[4]) + 264 | 1.0 * std::sin(q[1]) * std::sin(q[3]) * std::sin(q[5]) * std::cos(q[2]) * 265 | std::cos(q[6] + M_PI_4) - 266 | 1.0 * std::sin(q[1]) * std::sin(q[4]) * std::sin(q[6] + M_PI_4) * 267 | std::cos(q[2]) * std::cos(q[3]) - 268 | 1.0 * std::sin(q[1]) * std::cos(q[2]) * std::cos(q[3]) * std::cos(q[4]) * 269 | std::cos(q[5]) * std::cos(q[6] + M_PI_4) + 270 | 1.0 * std::sin(q[3]) * std::sin(q[4]) * std::sin(q[6] + M_PI_4) * 271 | std::cos(q[1]) + 272 | 1.0 * std::sin(q[3]) * std::cos(q[1]) * std::cos(q[4]) * std::cos(q[5]) * 273 | std::cos(q[6] + M_PI_4) + 274 | 1.0 * std::sin(q[5]) * std::cos(q[1]) * std::cos(q[3]) * 275 | std::cos(q[6] + M_PI_4); 276 | pose.row(2)(2) = -1.0 * 277 | ((std::sin(q[1]) * std::cos(q[2]) * std::cos(q[3]) - 278 | std::sin(q[3]) * std::cos(q[1])) * 279 | std::cos(q[4]) - 280 | std::sin(q[1]) * std::sin(q[2]) * std::sin(q[4])) * 281 | std::sin(q[5]) - 282 | 1.0 * 283 | (std::sin(q[1]) * std::sin(q[3]) * std::cos(q[2]) + 284 | std::cos(q[1]) * std::cos(q[3])) * 285 | std::cos(q[5]); 286 | pose.row(2)(3) = 287 | -0.21000000000000002 * 288 | ((std::sin(q[1]) * std::cos(q[2]) * std::cos(q[3]) - 289 | std::sin(q[3]) * std::cos(q[1])) * 290 | std::cos(q[4]) - 291 | std::sin(q[1]) * std::sin(q[2]) * std::sin(q[4])) * 292 | std::sin(q[5]) - 293 | 0.087999999999999995 * 294 | ((std::sin(q[1]) * std::cos(q[2]) * std::cos(q[3]) - 295 | std::sin(q[3]) * std::cos(q[1])) * 296 | std::cos(q[4]) - 297 | std::sin(q[1]) * std::sin(q[2]) * std::sin(q[4])) * 298 | std::cos(q[5]) + 299 | 0.087999999999999995 * 300 | (std::sin(q[1]) * std::sin(q[3]) * std::cos(q[2]) + 301 | std::cos(q[1]) * std::cos(q[3])) * 302 | std::sin(q[5]) - 303 | 0.21000000000000002 * 304 | (std::sin(q[1]) * std::sin(q[3]) * std::cos(q[2]) + 305 | std::cos(q[1]) * std::cos(q[3])) * 306 | std::cos(q[5]) + 307 | 0.38400000000000001 * std::sin(q[1]) * std::sin(q[3]) * std::cos(q[2]) + 308 | 0.082500000000000004 * std::sin(q[1]) * std::cos(q[2]) * std::cos(q[3]) - 309 | 0.082500000000000004 * std::sin(q[1]) * std::cos(q[2]) - 310 | 0.082500000000000004 * std::sin(q[3]) * std::cos(q[1]) + 311 | 0.38400000000000001 * std::cos(q[1]) * std::cos(q[3]) + 312 | 0.316 * std::cos(q[1]) + 0.33300000000000002; 313 | pose.row(3)(3) = 1.0; 314 | return pose; 315 | }; 316 | 317 | } // namespace kinematics 318 | -------------------------------------------------------------------------------- /include/kinematics/ik.h: -------------------------------------------------------------------------------- 1 | // Analytical Franka inverse kinematics using q7 as redundant parameter 2 | // - Yanhao He, February 2020 3 | 4 | #ifndef FRANKA_IK_HE_HPP 5 | #define FRANKA_IK_HE_HPP 6 | 7 | #include 8 | #include 9 | 10 | #include "Eigen/Dense" 11 | #include "constants.h" 12 | #include "utils.h" 13 | 14 | namespace kinematics { 15 | 16 | const double kQDefaultData[7] = {0.0, -M_PI_4, 0.0, -3 * M_PI_4, 17 | 0.0, M_PI_2, M_PI_4}; 18 | const Vector7d kQDefault(kQDefaultData); 19 | 20 | inline Eigen::Matrix ik_full(Eigen::Matrix O_T_EE, 21 | Vector7d q_actual_array = kQDefault, 22 | double q7 = M_PI_4) { 23 | const Eigen::Matrix q_all_NAN = 24 | Eigen::Matrix::Constant( 25 | std::numeric_limits::quiet_NaN()); 26 | const Vector7d q_NAN = 27 | Vector7d::Constant(std::numeric_limits::quiet_NaN()); 28 | Eigen::Matrix q_all = q_all_NAN; 29 | 30 | const double d1 = 0.3330; 31 | const double d3 = 0.3160; 32 | const double d5 = 0.3840; 33 | const double d7e = 0.2104; 34 | const double a4 = 0.0825; 35 | const double a7 = 0.0880; 36 | 37 | const double LL24 = 0.10666225; // a4^2 + d3^2 38 | const double LL46 = 0.15426225; // a4^2 + d5^2 39 | const double L24 = 0.326591870689; // sqrt(LL24) 40 | const double L46 = 0.392762332715; // sqrt(LL46) 41 | 42 | const double thetaH46 = 1.35916951803; // atan(d5/a4); 43 | const double theta342 = 1.31542071191; // atan(d3/a4); 44 | const double theta46H = 0.211626808766; // acot(d5/a4); 45 | 46 | const std::array q_min = { 47 | {-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973}}; 48 | const std::array q_max = { 49 | {2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973}}; 50 | 51 | if (q7 <= q_min[6] || q7 >= q_max[6]) 52 | return q_all_NAN; 53 | else 54 | for (int i = 0; i < 4; i++) q_all(i, 6) = q7; 55 | 56 | // compute p_6 57 | Eigen::Matrix3d R_EE = O_T_EE.topLeftCorner<3, 3>(); 58 | Eigen::Vector3d z_EE = O_T_EE.block<3, 1>(0, 2); 59 | Eigen::Vector3d p_EE = O_T_EE.block<3, 1>(0, 3); 60 | Eigen::Vector3d p_7 = p_EE - d7e * z_EE; 61 | 62 | Eigen::Vector3d x_EE_6; 63 | x_EE_6 << std::cos(q7 - M_PI_4), -std::sin(q7 - M_PI_4), 0.0; 64 | Eigen::Vector3d x_6 = R_EE * x_EE_6; 65 | x_6 /= x_6.norm(); // visibly increases accuracy 66 | Eigen::Vector3d p_6 = p_7 - a7 * x_6; 67 | 68 | // compute q4 69 | Eigen::Vector3d p_2; 70 | p_2 << 0.0, 0.0, d1; 71 | Eigen::Vector3d V26 = p_6 - p_2; 72 | 73 | double LL26 = V26[0] * V26[0] + V26[1] * V26[1] + V26[2] * V26[2]; 74 | double L26 = std::sqrt(LL26); 75 | 76 | if (L24 + L46 < L26 || L24 + L26 < L46 || L26 + L46 < L24) return q_all_NAN; 77 | 78 | double theta246 = std::acos((LL24 + LL46 - LL26) / 2.0 / L24 / L46); 79 | double q4 = theta246 + thetaH46 + theta342 - 2.0 * M_PI; 80 | if (q4 <= q_min[3] || q4 >= q_max[3]) 81 | return q_all_NAN; 82 | else 83 | for (int i = 0; i < 4; i++) q_all(i, 3) = q4; 84 | 85 | // compute q6 86 | double theta462 = std::acos((LL26 + LL46 - LL24) / 2.0 / L26 / L46); 87 | double theta26H = theta46H + theta462; 88 | double D26 = -L26 * std::cos(theta26H); 89 | 90 | Eigen::Vector3d Z_6 = z_EE.cross(x_6); 91 | Eigen::Vector3d Y_6 = Z_6.cross(x_6); 92 | Eigen::Matrix3d R_6; 93 | R_6.col(0) = x_6; 94 | R_6.col(1) = Y_6 / Y_6.norm(); 95 | R_6.col(2) = Z_6 / Z_6.norm(); 96 | Eigen::Vector3d V_6_62 = R_6.transpose() * (-V26); 97 | 98 | double Phi6 = std::atan2(V_6_62[1], V_6_62[0]); 99 | double Theta6 = 100 | std::asin(D26 / std::sqrt(V_6_62[0] * V_6_62[0] + V_6_62[1] * V_6_62[1])); 101 | 102 | std::array q6; 103 | q6[0] = M_PI - Theta6 - Phi6; 104 | q6[1] = Theta6 - Phi6; 105 | 106 | for (int i = 0; i < 2; i++) { 107 | if (q6[i] <= q_min[5]) 108 | q6[i] += 2.0 * M_PI; 109 | else if (q6[i] >= q_max[5]) 110 | q6[i] -= 2.0 * M_PI; 111 | 112 | if (q6[i] <= q_min[5] || q6[i] >= q_max[5]) { 113 | q_all.row(2 * i) = q_NAN; 114 | q_all.row(2 * i + 1) = q_NAN; 115 | } else { 116 | q_all(2 * i, 5) = q6[i]; 117 | q_all(2 * i + 1, 5) = q6[i]; 118 | } 119 | } 120 | if (std::isnan(q_all(0, 5)) && std::isnan(q_all(2, 5))) return q_all_NAN; 121 | 122 | // compute q1 & q2 123 | double thetaP26 = 3.0 * M_PI_2 - theta462 - theta246 - theta342; 124 | double thetaP = M_PI - thetaP26 - theta26H; 125 | double LP6 = L26 * sin(thetaP26) / std::sin(thetaP); 126 | 127 | std::array z_5_all; 128 | std::array V2P_all; 129 | 130 | for (int i = 0; i < 2; i++) { 131 | Eigen::Vector3d z_6_5; 132 | z_6_5 << std::sin(q6[i]), std::cos(q6[i]), 0.0; 133 | Eigen::Vector3d z_5 = R_6 * z_6_5; 134 | Eigen::Vector3d V2P = p_6 - LP6 * z_5 - p_2; 135 | 136 | z_5_all[2 * i] = z_5; 137 | z_5_all[2 * i + 1] = z_5; 138 | V2P_all[2 * i] = V2P; 139 | V2P_all[2 * i + 1] = V2P; 140 | 141 | double L2P = V2P.norm(); 142 | 143 | if (std::fabs(V2P[2] / L2P) > 0.999) { 144 | q_all(2 * i, 0) = q_actual_array[0]; 145 | q_all(2 * i, 1) = 0.0; 146 | q_all(2 * i + 1, 0) = q_actual_array[0]; 147 | q_all(2 * i + 1, 1) = 0.0; 148 | } else { 149 | q_all(2 * i, 0) = std::atan2(V2P[1], V2P[0]); 150 | q_all(2 * i, 1) = std::acos(V2P[2] / L2P); 151 | if (q_all(2 * i, 0) < 0) 152 | q_all(2 * i + 1, 0) = q_all(2 * i, 0) + M_PI; 153 | else 154 | q_all(2 * i + 1, 0) = q_all(2 * i, 0) - M_PI; 155 | q_all(2 * i + 1, 1) = -q_all(2 * i, 1); 156 | } 157 | } 158 | 159 | for (int i = 0; i < 4; i++) { 160 | if (q_all(i, 0) <= q_min[0] || q_all(i, 0) >= q_max[0] || 161 | q_all(i, 1) <= q_min[1] || q_all(i, 1) >= q_max[1]) { 162 | q_all.row(i) = q_NAN; 163 | continue; 164 | } 165 | 166 | // compute q3 167 | Eigen::Vector3d z_3 = V2P_all[i] / V2P_all[i].norm(); 168 | Eigen::Vector3d Y_3 = -V26.cross(V2P_all[i]); 169 | Eigen::Vector3d y_3 = Y_3 / Y_3.norm(); 170 | Eigen::Vector3d x_3 = y_3.cross(z_3); 171 | Eigen::Matrix3d R_1; 172 | double c1 = std::cos(q_all(i, 0)); 173 | double s1 = std::sin(q_all(i, 0)); 174 | R_1 << c1, -s1, 0.0, s1, c1, 0.0, 0.0, 0.0, 1.0; 175 | Eigen::Matrix3d R_1_2; 176 | double c2 = std::cos(q_all(i, 1)); 177 | double s2 = std::sin(q_all(i, 1)); 178 | R_1_2 << c2, -s2, 0.0, 0.0, 0.0, 1.0, -s2, -c2, 0.0; 179 | Eigen::Matrix3d R_2 = R_1 * R_1_2; 180 | Eigen::Vector3d x_2_3 = R_2.transpose() * x_3; 181 | q_all(i, 2) = std::atan2(x_2_3[2], x_2_3[0]); 182 | 183 | if (q_all(i, 2) <= q_min[2] || q_all(i, 2) >= q_max[2]) { 184 | q_all.row(i) = q_NAN; 185 | continue; 186 | } 187 | 188 | // compute q5 189 | Eigen::Vector3d VH4 = p_2 + d3 * z_3 + a4 * x_3 - p_6 + d5 * z_5_all[i]; 190 | Eigen::Matrix3d R_5_6; 191 | double c6 = std::cos(q_all(i, 5)); 192 | double s6 = std::sin(q_all(i, 5)); 193 | R_5_6 << c6, -s6, 0.0, 0.0, 0.0, -1.0, s6, c6, 0.0; 194 | Eigen::Matrix3d R_5 = R_6 * R_5_6.transpose(); 195 | Eigen::Vector3d V_5_H4 = R_5.transpose() * VH4; 196 | 197 | q_all(i, 4) = -std::atan2(V_5_H4[1], V_5_H4[0]); 198 | if (q_all(i, 4) <= q_min[4] || q_all(i, 4) >= q_max[4]) { 199 | q_all.row(i) = q_NAN; 200 | continue; 201 | } 202 | } 203 | 204 | return q_all; 205 | } 206 | 207 | inline Eigen::Matrix ik_full(const Eigen::Vector3d &position, 208 | const Eigen::Vector4d &orientation, 209 | Vector7d q_actual_array = kQDefault, 210 | double q7 = M_PI_4) { 211 | Eigen::Matrix T = 212 | PositionOrientationToMatrix(position, orientation); 213 | return ik_full(T, q_actual_array, q7); 214 | } 215 | 216 | inline Vector7d ik(Eigen::Matrix O_T_EE, 217 | Vector7d q_actual_array = kQDefault, double q7 = M_PI_4) { 218 | const Vector7d q_NAN = 219 | Vector7d::Constant(std::numeric_limits::quiet_NaN()); 220 | Vector7d q; 221 | 222 | // constants 223 | const double d1 = 0.3330; 224 | const double d3 = 0.3160; 225 | const double d5 = 0.3840; 226 | const double d7e = 0.2104; 227 | const double a4 = 0.0825; 228 | const double a7 = 0.0880; 229 | 230 | const double LL24 = 0.10666225; // a4^2 + d3^2 231 | const double LL46 = 0.15426225; // a4^2 + d5^2 232 | const double L24 = 0.326591870689; // sqrt(LL24) 233 | const double L46 = 0.392762332715; // sqrt(LL46) 234 | 235 | const double thetaH46 = 1.35916951803; // atan(d5/a4); 236 | const double theta342 = 1.31542071191; // atan(d3/a4); 237 | const double theta46H = 0.211626808766; // acot(d5/a4); 238 | 239 | const std::array q_min = { 240 | {-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973}}; 241 | const std::array q_max = { 242 | {2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973}}; 243 | 244 | // return NAN if input q7 is out of range 245 | if (q7 <= q_min[6] || q7 >= q_max[6]) 246 | return q_NAN; 247 | else 248 | q[6] = q7; 249 | 250 | // FK for getting current case id 251 | double c1_a = std::cos(q_actual_array[0]); 252 | double s1_a = std::sin(q_actual_array[0]); 253 | double c2_a = std::cos(q_actual_array[1]); 254 | double s2_a = std::sin(q_actual_array[1]); 255 | double c3_a = std::cos(q_actual_array[2]); 256 | double s3_a = std::sin(q_actual_array[2]); 257 | double c4_a = std::cos(q_actual_array[3]); 258 | double s4_a = std::sin(q_actual_array[3]); 259 | double c5_a = std::cos(q_actual_array[4]); 260 | double s5_a = std::sin(q_actual_array[4]); 261 | double c6_a = std::cos(q_actual_array[5]); 262 | double s6_a = std::sin(q_actual_array[5]); 263 | 264 | std::array, 7> As_a; 265 | As_a[0] << c1_a, -s1_a, 0.0, 0.0, // O1 266 | s1_a, c1_a, 0.0, 0.0, 0.0, 0.0, 1.0, d1, 0.0, 0.0, 0.0, 1.0; 267 | As_a[1] << c2_a, -s2_a, 0.0, 0.0, // O2 268 | 0.0, 0.0, 1.0, 0.0, -s2_a, -c2_a, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; 269 | As_a[2] << c3_a, -s3_a, 0.0, 0.0, // O3 270 | 0.0, 0.0, -1.0, -d3, s3_a, c3_a, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; 271 | As_a[3] << c4_a, -s4_a, 0.0, a4, // O4 272 | 0.0, 0.0, -1.0, 0.0, s4_a, c4_a, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; 273 | As_a[4] << 1.0, 0.0, 0.0, -a4, // H 274 | 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0; 275 | As_a[5] << c5_a, -s5_a, 0.0, 0.0, // O5 276 | 0.0, 0.0, 1.0, d5, -s5_a, -c5_a, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; 277 | As_a[6] << c6_a, -s6_a, 0.0, 0.0, // O6 278 | 0.0, 0.0, -1.0, 0.0, s6_a, c6_a, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; 279 | std::array, 7> Ts_a; 280 | Ts_a[0] = As_a[0]; 281 | for (unsigned int j = 1; j < 7; j++) Ts_a[j] = Ts_a[j - 1] * As_a[j]; 282 | 283 | // identify q6 case 284 | Eigen::Vector3d V62_a = Ts_a[1].block<3, 1>(0, 3) - Ts_a[6].block<3, 1>(0, 3); 285 | Eigen::Vector3d V6H_a = Ts_a[4].block<3, 1>(0, 3) - Ts_a[6].block<3, 1>(0, 3); 286 | Eigen::Vector3d Z6_a = Ts_a[6].block<3, 1>(0, 2); 287 | bool is_case6_0 = ((V6H_a.cross(V62_a)).transpose() * Z6_a <= 0); 288 | 289 | // identify q1 case 290 | bool is_case1_1 = (q_actual_array[1] < 0); 291 | 292 | // IK: compute p_6 293 | Eigen::Matrix3d R_EE = O_T_EE.topLeftCorner<3, 3>(); 294 | Eigen::Vector3d z_EE = O_T_EE.block<3, 1>(0, 2); 295 | Eigen::Vector3d p_EE = O_T_EE.block<3, 1>(0, 3); 296 | Eigen::Vector3d p_7 = p_EE - d7e * z_EE; 297 | 298 | Eigen::Vector3d x_EE_6; 299 | x_EE_6 << std::cos(q7 - M_PI_4), -std::sin(q7 - M_PI_4), 0.0; 300 | Eigen::Vector3d x_6 = R_EE * x_EE_6; 301 | x_6 /= x_6.norm(); // visibly increases accuracy 302 | Eigen::Vector3d p_6 = p_7 - a7 * x_6; 303 | 304 | // IK: compute q4 305 | Eigen::Vector3d p_2; 306 | p_2 << 0.0, 0.0, d1; 307 | Eigen::Vector3d V26 = p_6 - p_2; 308 | 309 | double LL26 = V26[0] * V26[0] + V26[1] * V26[1] + V26[2] * V26[2]; 310 | double L26 = std::sqrt(LL26); 311 | 312 | if (L24 + L46 < L26 || L24 + L26 < L46 || L26 + L46 < L24) return q_NAN; 313 | 314 | double theta246 = std::acos((LL24 + LL46 - LL26) / 2.0 / L24 / L46); 315 | q[3] = theta246 + thetaH46 + theta342 - 2.0 * M_PI; 316 | if (q[3] <= q_min[3] || q[3] >= q_max[3]) return q_NAN; 317 | 318 | // IK: compute q6 319 | double theta462 = std::acos((LL26 + LL46 - LL24) / 2.0 / L26 / L46); 320 | double theta26H = theta46H + theta462; 321 | double D26 = -L26 * std::cos(theta26H); 322 | 323 | Eigen::Vector3d Z_6 = z_EE.cross(x_6); 324 | Eigen::Vector3d Y_6 = Z_6.cross(x_6); 325 | Eigen::Matrix3d R_6; 326 | R_6.col(0) = x_6; 327 | R_6.col(1) = Y_6 / Y_6.norm(); 328 | R_6.col(2) = Z_6 / Z_6.norm(); 329 | Eigen::Vector3d V_6_62 = R_6.transpose() * (-V26); 330 | 331 | double Phi6 = std::atan2(V_6_62[1], V_6_62[0]); 332 | double Theta6 = 333 | std::asin(D26 / std::sqrt(V_6_62[0] * V_6_62[0] + V_6_62[1] * V_6_62[1])); 334 | 335 | if (is_case6_0) 336 | q[5] = M_PI - Theta6 - Phi6; 337 | else 338 | q[5] = Theta6 - Phi6; 339 | 340 | if (q[5] <= q_min[5]) 341 | q[5] += 2.0 * M_PI; 342 | else if (q[5] >= q_max[5]) 343 | q[5] -= 2.0 * M_PI; 344 | 345 | if (q[5] <= q_min[5] || q[5] >= q_max[5]) return q_NAN; 346 | 347 | // IK: compute q1 & q2 348 | double thetaP26 = 3.0 * M_PI_2 - theta462 - theta246 - theta342; 349 | double thetaP = M_PI - thetaP26 - theta26H; 350 | double LP6 = L26 * sin(thetaP26) / std::sin(thetaP); 351 | 352 | Eigen::Vector3d z_6_5; 353 | z_6_5 << std::sin(q[5]), std::cos(q[5]), 0.0; 354 | Eigen::Vector3d z_5 = R_6 * z_6_5; 355 | Eigen::Vector3d V2P = p_6 - LP6 * z_5 - p_2; 356 | 357 | double L2P = V2P.norm(); 358 | 359 | if (std::fabs(V2P[2] / L2P) > 0.999) { 360 | q[0] = q_actual_array[0]; 361 | q[1] = 0.0; 362 | } else { 363 | q[0] = std::atan2(V2P[1], V2P[0]); 364 | q[1] = std::acos(V2P[2] / L2P); 365 | if (is_case1_1) { 366 | if (q[0] < 0.0) 367 | q[0] += M_PI; 368 | else 369 | q[0] -= M_PI; 370 | q[1] = -q[1]; 371 | } 372 | } 373 | 374 | if (q[0] <= q_min[0] || q[0] >= q_max[0] || q[1] <= q_min[1] || 375 | q[1] >= q_max[1]) 376 | return q_NAN; 377 | 378 | // IK: compute q3 379 | Eigen::Vector3d z_3 = V2P / V2P.norm(); 380 | Eigen::Vector3d Y_3 = -V26.cross(V2P); 381 | Eigen::Vector3d y_3 = Y_3 / Y_3.norm(); 382 | Eigen::Vector3d x_3 = y_3.cross(z_3); 383 | Eigen::Matrix3d R_1; 384 | double c1 = std::cos(q[0]); 385 | double s1 = std::sin(q[0]); 386 | R_1 << c1, -s1, 0.0, s1, c1, 0.0, 0.0, 0.0, 1.0; 387 | Eigen::Matrix3d R_1_2; 388 | double c2 = std::cos(q[1]); 389 | double s2 = std::sin(q[1]); 390 | R_1_2 << c2, -s2, 0.0, 0.0, 0.0, 1.0, -s2, -c2, 0.0; 391 | Eigen::Matrix3d R_2 = R_1 * R_1_2; 392 | Eigen::Vector3d x_2_3 = R_2.transpose() * x_3; 393 | q[2] = std::atan2(x_2_3[2], x_2_3[0]); 394 | 395 | if (q[2] <= q_min[2] || q[2] >= q_max[2]) return q_NAN; 396 | 397 | // IK: compute q5 398 | Eigen::Vector3d VH4 = p_2 + d3 * z_3 + a4 * x_3 - p_6 + d5 * z_5; 399 | Eigen::Matrix3d R_5_6; 400 | double c6 = std::cos(q[5]); 401 | double s6 = std::sin(q[5]); 402 | R_5_6 << c6, -s6, 0.0, 0.0, 0.0, -1.0, s6, c6, 0.0; 403 | Eigen::Matrix3d R_5 = R_6 * R_5_6.transpose(); 404 | Eigen::Vector3d V_5_H4 = R_5.transpose() * VH4; 405 | 406 | q[4] = -std::atan2(V_5_H4[1], V_5_H4[0]); 407 | if (q[4] <= q_min[4] || q[4] >= q_max[4]) return q_NAN; 408 | 409 | return q; 410 | } 411 | 412 | inline Vector7d ik(const Eigen::Vector3d &position, 413 | const Eigen::Vector4d &orientation, 414 | Vector7d q_actual_array = kQDefault, double q7 = M_PI_4) { 415 | Eigen::Matrix T = 416 | PositionOrientationToMatrix(position, orientation); 417 | return ik(T, q_actual_array, q7); 418 | } 419 | 420 | } // namespace kinematics 421 | 422 | #endif // FRANKA_IK_HE_HPP -------------------------------------------------------------------------------- /include/motion/generators.h: -------------------------------------------------------------------------------- 1 | #ifndef PARABOLIC_BLEND_SMOOTHER_H_ 2 | #define PARABOLIC_BLEND_SMOOTHER_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "kinematics/ik.h" 12 | #include "motion/time_optimal/trajectory.h" 13 | #include "utils.h" 14 | 15 | namespace py = pybind11; 16 | namespace motion { 17 | 18 | const double kDefaultTimeout = 30.0; 19 | const double kDefaultJointSpeedFactor = 0.2; 20 | const double kDefaultCartesianSpeedFactor = 0.2; 21 | 22 | class PandaTrajectory { 23 | public: 24 | double getDuration() { return traj_->getDuration(); } 25 | 26 | protected: 27 | bool _computeTrajectory(const time_optimal::Path &path, 28 | const Eigen::VectorXd &max_velocity, 29 | const Eigen::VectorXd &max_acceleration, 30 | double timeout); 31 | 32 | template 33 | void _log(const std::string level, Args &&...args) { 34 | py::gil_scoped_acquire acquire; 35 | logger_.attr(level.c_str())(args...); 36 | } 37 | 38 | py::object logger_; 39 | std::shared_ptr traj_; 40 | }; 41 | 42 | class JointTrajectory : public PandaTrajectory { 43 | public: 44 | JointTrajectory(const std::vector &waypoints, 45 | double speed_factor = kDefaultJointSpeedFactor, 46 | double maxDeviation = 0.0, double timeout = kDefaultTimeout); 47 | 48 | Vector7d getJointPositions(double time); 49 | 50 | Vector7d getJointVelocities(double time); 51 | 52 | Vector7d getJointAccelerations(double time); 53 | 54 | private: 55 | time_optimal::Path _convertList(const std::vector &list, 56 | double maxDeviation = 0.0); 57 | }; 58 | 59 | class CartesianTrajectory : public PandaTrajectory { 60 | public: 61 | CartesianTrajectory( 62 | const std::vector> &positions, 63 | const std::vector> &orientations, 64 | double speed_factor = kDefaultCartesianSpeedFactor, 65 | double maxDeviation = 0.0, double timeout = kDefaultTimeout); 66 | 67 | CartesianTrajectory(const std::vector> &poses, 68 | double speed_factor = kDefaultCartesianSpeedFactor, 69 | double maxDeviation = 0.0, 70 | double timeout = kDefaultTimeout); 71 | 72 | Eigen::Matrix getPose(double time); 73 | 74 | Eigen::Vector3d getPosition(double time); 75 | 76 | Eigen::Vector4d getOrientation(double time); 77 | 78 | private: 79 | std::vector angles_; 80 | std::vector axes_; 81 | std::vector orientations_; 82 | }; 83 | 84 | } // namespace motion 85 | 86 | #endif -------------------------------------------------------------------------------- /include/motion/time_optimal/path.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2011, Georgia Tech Research Corporation 3 | * All rights reserved. 4 | * 5 | * Author: Tobias Kunz 6 | * Date: 05/2012 7 | * 8 | * Humanoid Robotics Lab Georgia Institute of Technology 9 | * Director: Mike Stilman http://www.golems.org 10 | * 11 | * Algorithm details and publications: 12 | * http://www.golems.org/node/1570 13 | * 14 | * This file is provided under the following "BSD-style" License: 15 | * Redistribution and use in source and binary forms, with or 16 | * without modification, are permitted provided that the following 17 | * conditions are met: 18 | * * Redistributions of source code must retain the above copyright 19 | * notice, this list of conditions and the following disclaimer. 20 | * * Redistributions in binary form must reproduce the above 21 | * copyright notice, this list of conditions and the following 22 | * disclaimer in the documentation and/or other materials provided 23 | * with the distribution. 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 25 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 26 | * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 27 | * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 29 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 30 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 31 | * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF 32 | * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 33 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 34 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 35 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 36 | * POSSIBILITY OF SUCH DAMAGE. 37 | */ 38 | 39 | #pragma once 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | namespace motion { 46 | namespace time_optimal { 47 | 48 | class PathSegment { 49 | public: 50 | PathSegment(double length = 0.0) : length_(length) {} 51 | virtual ~PathSegment() // is required for destructing derived classes 52 | {} 53 | double getLength() const { return length_; } 54 | virtual Eigen::VectorXd getConfig(double s) const = 0; 55 | virtual Eigen::VectorXd getTangent(double s) const = 0; 56 | virtual Eigen::VectorXd getCurvature(double s) const = 0; 57 | virtual std::list getSwitchingPoints() const = 0; 58 | virtual PathSegment* clone() const = 0; 59 | 60 | double position_; 61 | 62 | protected: 63 | double length_; 64 | }; 65 | 66 | class Path { 67 | public: 68 | Path(const std::list& path, double max_deviation = 0.0); 69 | Path(const Path& path); 70 | double getLength() const; 71 | Eigen::VectorXd getConfig(double s) const; 72 | Eigen::VectorXd getTangent(double s) const; 73 | Eigen::VectorXd getCurvature(double s) const; 74 | 75 | /** @brief Get the next switching point. 76 | * @param[in] s Arc length traveled so far 77 | * @param[out] discontinuity True if this switching point is a discontinuity 78 | * @return arc length to the switching point 79 | **/ 80 | double getNextSwitchingPoint(double s, bool& discontinuity) const; 81 | 82 | /// @brief Return a list of all switching points as a pair (arc length to 83 | /// switching point, discontinuity) 84 | std::list> getSwitchingPoints() const; 85 | 86 | std::list section_lengths; 87 | 88 | private: 89 | PathSegment* getPathSegment(double& s) const; 90 | double length_; 91 | std::list> switching_points_; 92 | std::list> path_segments_; 93 | }; 94 | 95 | } // namespace time_optimal 96 | } // namespace motion 97 | -------------------------------------------------------------------------------- /include/motion/time_optimal/trajectory.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Georgia Tech Research Corporation 3 | * All rights reserved. 4 | * 5 | * Author: Tobias Kunz 6 | * Date: 05/2012 7 | * 8 | * Humanoid Robotics Lab Georgia Institute of Technology 9 | * Director: Mike Stilman http://www.golems.org 10 | * 11 | * Algorithm details and publications: 12 | * http://www.golems.org/node/1570 13 | * 14 | * This file is provided under the following "BSD-style" License: 15 | * Redistribution and use in source and binary forms, with or 16 | * without modification, are permitted provided that the following 17 | * conditions are met: 18 | * * Redistributions of source code must retain the above copyright 19 | * notice, this list of conditions and the following disclaimer. 20 | * * Redistributions in binary form must reproduce the above 21 | * copyright notice, this list of conditions and the following 22 | * disclaimer in the documentation and/or other materials provided 23 | * with the distribution. 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 25 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 26 | * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 27 | * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 29 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 30 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 31 | * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF 32 | * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 33 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 34 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 35 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 36 | * POSSIBILITY OF SUCH DAMAGE. 37 | */ 38 | 39 | #pragma once 40 | 41 | #include 42 | #include 43 | #include "motion/time_optimal/path.h" 44 | 45 | namespace py = pybind11; 46 | 47 | namespace motion { 48 | namespace time_optimal { 49 | 50 | class Trajectory { 51 | public: 52 | /// @brief Generates a time-optimal trajectory 53 | Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, 54 | const Eigen::VectorXd& max_acceleration, double time_step = 0.001); 55 | 56 | ~Trajectory(); 57 | 58 | /** @brief Call this method after constructing the object to make sure the 59 | trajectory generation succeeded without errors. If this method returns 60 | false, all other methods have undefined behavior. **/ 61 | bool isValid() const; 62 | 63 | /// @brief Returns the optimal duration of the trajectory 64 | double getDuration() const; 65 | 66 | size_t getTrajectorySegmentIndex(double time); 67 | 68 | /** @brief Return the position/configuration vector for a given point in time 69 | */ 70 | Eigen::VectorXd getPosition(double time) const; 71 | /** @brief Return the velocity vector for a given point in time */ 72 | Eigen::VectorXd getVelocity(double time) const; 73 | /** @brief Return the acceleration vector for a given point in time */ 74 | Eigen::VectorXd getAcceleration(double time) const; 75 | 76 | private: 77 | struct TrajectoryStep { 78 | TrajectoryStep() {} 79 | TrajectoryStep(double path_pos, double path_vel) 80 | : path_pos_(path_pos), path_vel_(path_vel) {} 81 | double path_pos_; 82 | double path_vel_; 83 | double time_; 84 | }; 85 | 86 | bool getNextSwitchingPoint(double path_pos, 87 | TrajectoryStep& next_switching_point, 88 | double& before_acceleration, 89 | double& after_acceleration); 90 | bool getNextAccelerationSwitchingPoint(double path_pos, 91 | TrajectoryStep& next_switching_point, 92 | double& before_acceleration, 93 | double& after_acceleration); 94 | bool getNextVelocitySwitchingPoint(double path_pos, 95 | TrajectoryStep& next_switching_point, 96 | double& before_acceleration, 97 | double& after_acceleration); 98 | bool integrateForward(std::list& trajectory, 99 | double acceleration); 100 | void integrateBackward(std::list& start_trajectory, 101 | double path_pos, double path_vel, double acceleration); 102 | double getMinMaxPathAcceleration(double path_position, double path_velocity, 103 | bool max); 104 | double getMinMaxPhaseSlope(double path_position, double path_velocity, 105 | bool max); 106 | double getAccelerationMaxPathVelocity(double path_pos) const; 107 | double getVelocityMaxPathVelocity(double path_pos) const; 108 | double getAccelerationMaxPathVelocityDeriv(double path_pos); 109 | double getVelocityMaxPathVelocityDeriv(double path_pos); 110 | 111 | std::list::const_iterator getTrajectorySegment( 112 | double time) const; 113 | 114 | Path path_; 115 | Eigen::VectorXd max_velocity_; 116 | Eigen::VectorXd max_acceleration_; 117 | unsigned int joint_num_; 118 | bool valid_; 119 | std::list trajectory_; 120 | std::list 121 | end_trajectory_; // non-empty only if the trajectory generation failed. 122 | 123 | const double time_step_; 124 | 125 | mutable double cached_time_; 126 | mutable std::list::const_iterator cached_trajectory_segment_; 127 | py::object logger_; 128 | }; 129 | 130 | } // namespace time_optimal 131 | } // namespace motion -------------------------------------------------------------------------------- /include/panda.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include "controllers/controller.h" 14 | #include "controllers/joint_limits/virtual_wall_controller.h" 15 | #include "controllers/joint_trajectory.h" 16 | #include "controllers/cartesian_trajectory.h" 17 | #include "controllers/applied_torque.h" 18 | #include "utils.h" 19 | 20 | namespace py = pybind11; 21 | 22 | class Panda; 23 | 24 | class PandaContext { 25 | public: 26 | PandaContext(Panda &panda, const double &frequency, const double &t_max = 0, 27 | const uint64_t &max_ticks = 0); 28 | const PandaContext &enter(); 29 | bool exit(const py::object &type, const py::object &value, 30 | const py::object &traceback); 31 | bool ok(); 32 | uint64_t getNumTicks(); 33 | double getTime(); 34 | 35 | private: 36 | double dt_, t_max_; 37 | uint64_t num_ticks_, max_ticks_; 38 | std::chrono::high_resolution_clock::time_point t_start_, t_prev_; 39 | Panda &panda_; 40 | }; 41 | 42 | class Panda { 43 | public: 44 | static const double kMoveToJointPositionThreshold; 45 | static const Vector7d kDefaultTeachingDamping; 46 | Panda( 47 | std::string hostname, std::string name = "panda", 48 | franka::RealtimeConfig realtime_config = franka::RealtimeConfig::kIgnore); 49 | ~Panda(); 50 | const PandaContext createContext(double frequency, double max_runtime = 0.0, 51 | uint64_t max_iter = 0); 52 | franka::Robot &getRobot(); 53 | franka::Model &getModel(); 54 | franka::RobotState getState(); 55 | void startController(std::shared_ptr controller); 56 | void stopController(); 57 | void enableLogging(size_t buffer_size); 58 | void disableLogging(); 59 | std::map> getLog(); 60 | bool moveToJointPosition( 61 | const Vector7d &position, 62 | double speed_factor = motion::kDefaultJointSpeedFactor, 63 | const Vector7d &stiffness = controllers::JointTrajectory::kDefaultStiffness, 64 | const Vector7d &damping = controllers::JointTrajectory::kDefaultDamping, 65 | double dq_threshold = controllers::JointTrajectory::kDefaultDqThreshold, 66 | double success_threshold = kMoveToJointPositionThreshold); 67 | bool moveToJointPosition( 68 | std::vector &waypoints, 69 | double speed_factor = motion::kDefaultJointSpeedFactor, 70 | const Vector7d &stiffness = controllers::JointTrajectory::kDefaultStiffness, 71 | const Vector7d &damping = controllers::JointTrajectory::kDefaultDamping, 72 | double dq_threshold = controllers::JointTrajectory::kDefaultDqThreshold, 73 | double success_threshold = kMoveToJointPositionThreshold); 74 | bool moveToPose( 75 | std::vector &positions, 76 | std::vector> &orientations, 77 | double speed_factor = motion::kDefaultCartesianSpeedFactor, 78 | const Eigen::Matrix &impedance = controllers::CartesianTrajectory::kDefaultImpedance, 79 | const double &damping_ratio = controllers::CartesianTrajectory::kDefaultDampingRatio, 80 | const double &nullspace_stiffness = controllers::CartesianTrajectory::kDefaultNullspaceStiffness, 81 | double dq_threshold = controllers::CartesianTrajectory::kDefaultDqThreshold, 82 | double success_threshold = kMoveToJointPositionThreshold); 83 | bool moveToPose( 84 | const Eigen::Vector3d &position, 85 | const Eigen::Matrix &orientation, 86 | double speed_factor = motion::kDefaultCartesianSpeedFactor, 87 | const Eigen::Matrix &impedance = controllers::CartesianTrajectory::kDefaultImpedance, 88 | const double &damping_ratio = controllers::CartesianTrajectory::kDefaultDampingRatio, 89 | const double &nullspace_stiffness = controllers::CartesianTrajectory::kDefaultNullspaceStiffness, 90 | double dq_threshold = controllers::CartesianTrajectory::kDefaultDqThreshold, 91 | double success_threshold = kMoveToJointPositionThreshold); 92 | bool moveToPose( 93 | const std::vector> &poses, 94 | double speed_factor = motion::kDefaultCartesianSpeedFactor, 95 | const Eigen::Matrix &impedance = controllers::CartesianTrajectory::kDefaultImpedance, 96 | const double &damping_ratio = controllers::CartesianTrajectory::kDefaultDampingRatio, 97 | const double &nullspace_stiffness = controllers::CartesianTrajectory::kDefaultNullspaceStiffness, 98 | double dq_threshold = controllers::CartesianTrajectory::kDefaultDqThreshold, 99 | double success_threshold = kMoveToJointPositionThreshold); 100 | bool moveToPose( 101 | const Eigen::Matrix &pose, 102 | double speed_factor = motion::kDefaultCartesianSpeedFactor, 103 | const Eigen::Matrix &impedance = controllers::CartesianTrajectory::kDefaultImpedance, 104 | const double &damping_ratio = controllers::CartesianTrajectory::kDefaultDampingRatio, 105 | const double &nullspace_stiffness = controllers::CartesianTrajectory::kDefaultNullspaceStiffness, 106 | double dq_threshold = controllers::CartesianTrajectory::kDefaultDqThreshold, 107 | double success_threshold = kMoveToJointPositionThreshold); 108 | bool moveToStart( 109 | double speed_factor = motion::kDefaultJointSpeedFactor, 110 | const Vector7d &stiffness = controllers::JointTrajectory::kDefaultStiffness, 111 | const Vector7d &damping = controllers::JointTrajectory::kDefaultDamping, 112 | double dq_threshold = controllers::JointTrajectory::kDefaultDqThreshold, 113 | double success_threshold = kMoveToJointPositionThreshold); 114 | Eigen::Vector3d getPosition(); 115 | Eigen::Vector4d getOrientation(bool scalar_first = false); 116 | Eigen::Vector4d getOrientationScalarLast(); 117 | Eigen::Vector4d getOrientationScalarFirst(); 118 | Vector7d getJointPositions(); 119 | Eigen::Matrix getPose(); 120 | void setDefaultBehavior(); 121 | void raiseError(); 122 | void recover(); 123 | void teaching_mode(bool active, const Vector7d &damping = kDefaultTeachingDamping); 124 | 125 | const std::string name_; 126 | 127 | private: 128 | void _startController(std::shared_ptr controller); 129 | void _runController(TorqueCallback &control); 130 | void _setState(const franka::RobotState &state); 131 | template 132 | void _log(const std::string level, Args &&...args); 133 | TorqueCallback _createTorqueCallback(); 134 | std::shared_ptr robot_; 135 | std::shared_ptr model_; 136 | franka::RobotState state_; 137 | std::mutex mux_; 138 | std::shared_ptr current_controller_; 139 | std::thread current_thread_; 140 | std::shared_ptr 141 | virtual_walls_; 142 | py::object logger_; 143 | std::string hostname_; 144 | std::shared_ptr last_error_; 145 | std::deque log_; 146 | bool log_enabled_ = false; 147 | size_t log_size_; 148 | }; -------------------------------------------------------------------------------- /include/utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include // for std::abs 8 | #include "constants.h" 9 | 10 | typedef Eigen::Matrix Vector6d; 11 | typedef Eigen::Matrix Vector7d; 12 | typedef std::array Array7d; 13 | 14 | template 15 | using Callback = 16 | std::function; 17 | typedef Callback TorqueCallback; 18 | 19 | template 20 | inline Eigen::Matrix ArrayToVector( 21 | const std::array& array) { 22 | Eigen::Matrix matrix; 23 | std::copy(array.begin(), array.end(), matrix.data()); 24 | return matrix; 25 | } 26 | 27 | template 28 | inline std::array VectorToArray( 29 | const Eigen::Matrix& matrix) { 30 | std::array array; 31 | std::copy(matrix.data(), matrix.data() + T, array.data()); 32 | return array; 33 | } 34 | 35 | template 36 | inline std::array VectorToArray(const Eigen::VectorXd& vector) { 37 | std::array array; 38 | std::copy(vector.data(), vector.data() + T, array.data()); 39 | return array; 40 | } 41 | 42 | inline Eigen::Matrix4d PositionOrientationToMatrix(const Eigen::Vector3d &position, const Eigen::Vector4d &orientation) { 43 | Eigen::Quaterniond q(orientation); 44 | Eigen::Affine3d transform; 45 | transform = q; 46 | transform.translation() = position; 47 | return transform.matrix(); 48 | } 49 | 50 | inline Eigen::Vector3d MatrixToPosition(const Eigen::Matrix4d &matrix) { 51 | Eigen::Affine3d transform(matrix); 52 | return transform.translation(); 53 | } 54 | 55 | inline Eigen::Vector4d MatrixToOrientation(const Eigen::Matrix4d &matrix) { 56 | Eigen::Affine3d transform(matrix); 57 | Eigen::Quaterniond q(transform.rotation()); 58 | return q.coeffs(); 59 | } 60 | 61 | inline void pseudoInverse(const Eigen::MatrixXd& M_, Eigen::MatrixXd& M_pinv_, 62 | bool damped = true) { 63 | double lambda_ = damped ? 0.2 : 0.0; 64 | 65 | Eigen::JacobiSVD svd( 66 | M_, Eigen::ComputeFullU | Eigen::ComputeFullV); 67 | Eigen::JacobiSVD::SingularValuesType sing_vals_ = 68 | svd.singularValues(); 69 | Eigen::MatrixXd S_ = 70 | M_; // copying the dimensions of M_, its content is not needed. 71 | S_.setZero(); 72 | 73 | for (int i = 0; i < sing_vals_.size(); i++) 74 | S_(i, i) = 75 | (sing_vals_(i)) / (sing_vals_(i) * sing_vals_(i) + lambda_ * lambda_); 76 | 77 | M_pinv_ = Eigen::MatrixXd(svd.matrixV() * S_.transpose() * 78 | svd.matrixU().transpose()); 79 | } 80 | 81 | inline double kDeltaTauMax = 1.0; 82 | 83 | inline Array7d saturateTorqueRate(const Array7d& tau_d_calculated, 84 | const Array7d& tau_J_d) { 85 | Array7d tau_d_saturated{}; 86 | for (size_t i = 0; i < 7; i++) { 87 | double difference = tau_d_calculated[i] - tau_J_d[i]; 88 | tau_d_saturated[i] = 89 | tau_J_d[i] + 90 | std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax); 91 | } 92 | return tau_d_saturated; 93 | } 94 | 95 | inline Array7d clipTorques(const Array7d& tau_d_calculdated) { 96 | Array7d tau_d_clipped{}; 97 | for (size_t i = 0; i < 7; i++) { 98 | tau_d_clipped[i] = std::max(std::min(tau_d_calculdated[i], kTauJMax[i]), -kTauJMax[i]); 99 | } 100 | return tau_d_clipped; 101 | } 102 | 103 | template 104 | T ema_filter(const T& value_f, const T& value, double alpha, 105 | bool rounding = false, double threshold = 1e-20); 106 | 107 | // Template definition for the general case, i.e. Eigen::Matrix 108 | template 109 | inline EigenMatrix ema_filter(const EigenMatrix& value_f, 110 | const EigenMatrix& value, double alpha, 111 | bool rounding, double threshold) { 112 | return value_f.binaryExpr( 113 | value, [alpha, rounding, threshold](const auto v_f, const auto v) { 114 | return ema_filter(v_f, v, alpha, rounding, threshold); 115 | }); 116 | } 117 | 118 | // Template specialization for double 119 | template <> 120 | inline double ema_filter(const double& value_f, const double& value, 121 | double alpha, bool rounding, 122 | double threshold) { 123 | if (rounding && std::abs(value - value_f) < threshold) { 124 | return value; 125 | } 126 | return alpha * value + (1 - alpha) * value_f; 127 | } -------------------------------------------------------------------------------- /logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JeanElsner/panda-py/c480378d84e403091b639b46b1aa95eac61da282/logo.png -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = [ "scikit-build-core", "pybind11@git+https://github.com/pybind/pybind11.git@master"] 3 | build-backend = "scikit_build_core.build" 4 | 5 | [project] 6 | name = "panda-python" 7 | version = "0.8.1" 8 | description = "Python bindings for the Panda robot" 9 | requires-python = ">=3.7" 10 | dependencies = ["websockets>=11.0", "requests", "numpy"] 11 | authors = [ 12 | { name = "Jean Elsner", email = "jean.elsner@tum.de" }, 13 | ] 14 | license = {file = "LICENSE"} 15 | readme = "README.md" 16 | keywords = ["python", "real-time", "control", "robot", "franka", "emika"] 17 | classifiers = [ 18 | "Intended Audience :: Science/Research", 19 | "Intended Audience :: Developers", 20 | "License :: OSI Approved :: Apache Software License", 21 | "Topic :: Scientific/Engineering", 22 | "Programming Language :: Python :: 3.7", 23 | "Programming Language :: Python :: 3.8", 24 | "Programming Language :: Python :: 3.9", 25 | "Programming Language :: Python :: 3.10", 26 | "Programming Language :: Python :: 3.11", 27 | "Programming Language :: Python :: 3.12", 28 | ] 29 | 30 | [project.optional-dependencies] 31 | examples = [ 32 | "roboticstoolbox-python", 33 | "matplotlib", 34 | "spatialmath-python", 35 | "ansitable", 36 | "qpsolvers", 37 | ] 38 | docs = [ 39 | "furo", 40 | "sphinx-reredirects", 41 | "sphinx", 42 | ] 43 | dev = [ 44 | "pybind11-stubgen", 45 | ] 46 | 47 | [project.scripts] 48 | panda-lock = "panda_py.cli:lock" 49 | panda-unlock = "panda_py.cli:unlock" 50 | panda-reboot = "panda_py.cli:reboot" 51 | panda-take-control = "panda_py.cli:take_control" 52 | panda-release-control = "panda_py.cli:release_control" 53 | 54 | [tool.cibuildwheel] 55 | manylinux-x86_64-image = "manylinux2014" 56 | build = [ "cp37-*", "cp38-*", "cp39-*", "cp310-*", "cp311-*", "cp312-*",] 57 | skip = [ "pp*", "*musllinux*",] 58 | environment = "LIBFRANKA_VER=0.9.2" 59 | 60 | [tool.cibuildwheel.linux] 61 | before-all = [ "./bin/before_install_centos.sh",] 62 | archs = [ "x86_64",] 63 | 64 | [tool.scikit-build.cmake] 65 | build-type = "Release" 66 | 67 | [tool.scikit-build.wheel] 68 | packages = ["src/panda_py"] 69 | 70 | [tool.scikit-build.cmake.define] 71 | VACUUM_GRIPPER = "ON" 72 | -------------------------------------------------------------------------------- /src/controllers/applied_force.cpp: -------------------------------------------------------------------------------- 1 | #include "controllers/applied_force.h" 2 | 3 | #include 4 | 5 | #include "panda.h" 6 | 7 | const double kDefaultDampingData[7] = {0, 0, 0, 0, 0, 0, 0}; 8 | const Vector7d AppliedForce::kDefaultDamping = Vector7d(kDefaultDampingData); 9 | const double AppliedForce::kDefaultFilterCoeff = 1.0; 10 | 11 | AppliedForce::AppliedForce(const Vector7d &damping, const double filter_coeff) 12 | : filter_coeff_(filter_coeff), K_d_(damping), K_d_target_(damping){}; 13 | 14 | franka::Torques AppliedForce::step(const franka::RobotState &robot_state, 15 | franka::Duration &duration) { 16 | // Observed states 17 | Vector6d f_d; 18 | Vector7d dq, tau_d, K_d; 19 | dq = Eigen::Map(robot_state.dq.data()); 20 | // These quantities may be modified outside of the control loop 21 | mux_.lock(); 22 | _updateFilter(); 23 | K_d = K_d_; 24 | f_d = f_d_; 25 | mux_.unlock(); 26 | std::array jacobian_array = 27 | model_->zeroJacobian(franka::Frame::kEndEffector, robot_state); 28 | Eigen::Map> jacobian(jacobian_array.data()); 29 | // Apply torque and damping 30 | tau_d << jacobian.transpose()*f_d - K_d.asDiagonal() * dq; 31 | 32 | franka::Torques torques = VectorToArray(tau_d); 33 | torques.motion_finished = motion_finished_; 34 | return torques; 35 | } 36 | 37 | void AppliedForce::_updateFilter() { 38 | f_d_ = ema_filter(f_d_, f_d_target_, filter_coeff_, true); 39 | K_d_ = ema_filter(K_d_, K_d_target_, filter_coeff_, true); 40 | } 41 | 42 | void AppliedForce::setControl(const Vector6d &force) { 43 | std::lock_guard lock(mux_); 44 | f_d_target_ = force; 45 | } 46 | 47 | void AppliedForce::setDamping(const Vector7d &damping) { 48 | std::lock_guard lock(mux_); 49 | K_d_target_ = damping; 50 | } 51 | 52 | void AppliedForce::setFilter(const double filter_coeff) { 53 | std::lock_guard lock(mux_); 54 | filter_coeff_ = filter_coeff; 55 | } 56 | 57 | void AppliedForce::start(const franka::RobotState &robot_state, 58 | std::shared_ptr model) { 59 | motion_finished_ = false; 60 | f_d_.setZero(); 61 | f_d_target_.setZero(); 62 | model_ = model; 63 | } 64 | 65 | void AppliedForce::stop(const franka::RobotState &robot_state, 66 | std::shared_ptr model) { 67 | motion_finished_ = true; 68 | } 69 | 70 | bool AppliedForce::isRunning() { return !motion_finished_; } 71 | 72 | const std::string AppliedForce::name() { return "Applied Force"; } -------------------------------------------------------------------------------- /src/controllers/applied_torque.cpp: -------------------------------------------------------------------------------- 1 | #include "controllers/applied_torque.h" 2 | 3 | #include 4 | 5 | #include "panda.h" 6 | 7 | const double kDefaultDampingData[7] = {0, 0, 0, 0, 0, 0, 0}; 8 | const Vector7d AppliedTorque::kDefaultDamping = Vector7d(kDefaultDampingData); 9 | const double AppliedTorque::kDefaultFilterCoeff = 1.0; 10 | 11 | AppliedTorque::AppliedTorque(const Vector7d &damping, const double filter_coeff) 12 | : filter_coeff_(filter_coeff), K_d_(damping), K_d_target_(damping){}; 13 | 14 | franka::Torques AppliedTorque::step(const franka::RobotState &robot_state, 15 | franka::Duration &duration) { 16 | // Observed states 17 | Vector7d dq, tau_d, K_d; 18 | dq = Eigen::Map(robot_state.dq.data()); 19 | // These quantities may be modified outside of the control loop 20 | mux_.lock(); 21 | _updateFilter(); 22 | K_d = K_d_; 23 | tau_d = tau_d_; 24 | mux_.unlock(); 25 | // Apply torque and damping 26 | tau_d << tau_d - K_d.asDiagonal() * dq; 27 | 28 | franka::Torques torques = VectorToArray(tau_d); 29 | torques.motion_finished = motion_finished_; 30 | return torques; 31 | } 32 | 33 | void AppliedTorque::_updateFilter() { 34 | tau_d_ = ema_filter(tau_d_, tau_d_target_, filter_coeff_, true); 35 | K_d_ = ema_filter(K_d_, K_d_target_, filter_coeff_, true); 36 | } 37 | 38 | void AppliedTorque::setControl(const Vector7d &torque) { 39 | std::lock_guard lock(mux_); 40 | tau_d_target_ = torque; 41 | } 42 | 43 | void AppliedTorque::setDamping(const Vector7d &damping) { 44 | std::lock_guard lock(mux_); 45 | K_d_target_ = damping; 46 | } 47 | 48 | void AppliedTorque::setFilter(const double filter_coeff) { 49 | std::lock_guard lock(mux_); 50 | filter_coeff_ = filter_coeff; 51 | } 52 | 53 | void AppliedTorque::start(const franka::RobotState &robot_state, 54 | std::shared_ptr model) { 55 | motion_finished_ = false; 56 | tau_d_.setZero(); 57 | tau_d_target_.setZero(); 58 | } 59 | 60 | void AppliedTorque::stop(const franka::RobotState &robot_state, 61 | std::shared_ptr model) { 62 | motion_finished_ = true; 63 | } 64 | 65 | bool AppliedTorque::isRunning() { return !motion_finished_; } 66 | 67 | const std::string AppliedTorque::name() { return "Applied Torque"; } -------------------------------------------------------------------------------- /src/controllers/cartesian_impedance.cpp: -------------------------------------------------------------------------------- 1 | #include "controllers/cartesian_impedance.h" 2 | 3 | #include 4 | 5 | #include "panda.h" 6 | 7 | // clang-format off 8 | double data[36] = {600, 0, 0, 0, 0, 0, 9 | 0, 600, 0, 0, 0, 0, 10 | 0, 0, 600, 0, 0, 0, 11 | 0, 0, 0, 30, 0, 0, 12 | 0, 0, 0, 0, 30, 0, 13 | 0, 0, 0, 0, 0, 30}; 14 | // clang-format on 15 | const Eigen::Matrix CartesianImpedance::kDefaultImpedance = 16 | Eigen::Matrix(data); 17 | const double CartesianImpedance::kDefaultDampingRatio = 1.0; 18 | const double CartesianImpedance::kDefaultNullspaceStiffness = 0.5; 19 | const double CartesianImpedance::kDefaultFilterCoeff = 1.0; 20 | 21 | CartesianImpedance::CartesianImpedance( 22 | const Eigen::Matrix &impedance, const double &damping_ratio, 23 | const double &nullspace_stiffness, const double &filter_coeff) { 24 | K_p_ = impedance; 25 | K_p_target_ = impedance; 26 | damping_ratio_ = damping_ratio; 27 | _computeDamping(); 28 | K_d_ = K_d_target_; 29 | nullspace_stiffness_ = nullspace_stiffness; 30 | nullspace_stiffnes_target_ = nullspace_stiffness; 31 | filter_coeff_ = filter_coeff; 32 | }; 33 | 34 | void CartesianImpedance::_computeDamping() { 35 | K_d_target_ = damping_ratio_ * 2 * K_p_.cwiseSqrt(); 36 | }; 37 | 38 | franka::Torques CartesianImpedance::step(const franka::RobotState &robot_state, 39 | franka::Duration &duration) { 40 | Eigen::Vector3d position_d; 41 | Eigen::Quaterniond orientation_d; 42 | Vector7d q_nullspace_d; 43 | Eigen::Matrix K_p, K_d; 44 | // These quantities may be modified outside of the control loop 45 | mux_.lock(); 46 | _updateFilter(); 47 | K_p = K_p_; 48 | K_d = K_d_; 49 | position_d = position_d_; 50 | orientation_d = orientation_d_; 51 | q_nullspace_d = q_nullspace_d_; 52 | mux_.unlock(); 53 | 54 | // get state variables 55 | std::array coriolis_array = model_->coriolis(robot_state); 56 | std::array jacobian_array = 57 | model_->zeroJacobian(franka::Frame::kEndEffector, robot_state); 58 | 59 | // convert to Eigen 60 | Eigen::Map coriolis(coriolis_array.data()); 61 | Eigen::Map> jacobian(jacobian_array.data()); 62 | Vector7d q = Eigen::Map(robot_state.q.data()); 63 | Vector7d dq = Eigen::Map(robot_state.dq.data()); 64 | Vector7d tau_J_d = Eigen::Map(robot_state.tau_J_d.data()); 65 | Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data())); 66 | Eigen::Vector3d position(transform.translation()); 67 | Eigen::Quaterniond orientation(transform.rotation()); 68 | 69 | // compute error to desired pose 70 | // position error 71 | Eigen::Matrix error; 72 | error.head(3) << position - position_d; 73 | 74 | // orientation error 75 | if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) { 76 | orientation.coeffs() << -orientation.coeffs(); 77 | } 78 | // "difference" quaternion 79 | Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d); 80 | error.tail(3) << error_quaternion.x(), error_quaternion.y(), 81 | error_quaternion.z(); 82 | // Transform to base frame 83 | error.tail(3) << -transform.rotation() * error.tail(3); 84 | 85 | // compute control 86 | // allocate variables 87 | Eigen::VectorXd tau_task(7), tau_nullspace(7), tau_d(7); 88 | 89 | // pseudoinverse for nullspace handling 90 | // kinematic pseuoinverse 91 | Eigen::MatrixXd jacobian_transpose_pinv; 92 | pseudoInverse(jacobian.transpose(), jacobian_transpose_pinv); 93 | 94 | // Cartesian PD control with damping ratio = 1 95 | tau_task << jacobian.transpose() * (-K_p * error - K_d * (jacobian * dq)); 96 | // nullspace PD control with damping ratio = 1 97 | tau_nullspace << (Eigen::MatrixXd::Identity(7, 7) - 98 | jacobian.transpose() * jacobian_transpose_pinv) * 99 | (nullspace_stiffness_ * (q_nullspace_d - q) - 100 | (2.0 * sqrt(nullspace_stiffness_)) * dq); 101 | // Desired torque 102 | tau_d << tau_task + tau_nullspace + coriolis; 103 | 104 | franka::Torques torques = VectorToArray<7>(tau_d); 105 | torques.motion_finished = motion_finished_; 106 | return torques; 107 | } 108 | 109 | void CartesianImpedance::_updateFilter() { 110 | K_p_ = ema_filter(K_p_, K_p_target_, filter_coeff_, true); 111 | K_d_ = ema_filter(K_d_, K_d_target_, filter_coeff_, true); 112 | nullspace_stiffness_ = ema_filter( 113 | nullspace_stiffness_, nullspace_stiffnes_target_, filter_coeff_, true); 114 | position_d_ = 115 | ema_filter(position_d_, position_d_target_, filter_coeff_, true); 116 | orientation_d_ = orientation_d_.slerp(filter_coeff_, orientation_d_target_); 117 | } 118 | 119 | void CartesianImpedance::setControl(const Eigen::Vector3d &position, 120 | const Eigen::Vector4d &orientation, 121 | const Vector7d &q_nullspace) { 122 | std::lock_guard lock(mux_); 123 | position_d_target_ = position; 124 | orientation_d_target_ = orientation; 125 | q_nullspace_d_target_ = q_nullspace; 126 | } 127 | 128 | void CartesianImpedance::setImpedance( 129 | const Eigen::Matrix &impedance) { 130 | std::lock_guard lock(mux_); 131 | K_p_target_ = impedance; 132 | _computeDamping(); 133 | } 134 | 135 | void CartesianImpedance::setDampingRatio(const double &damping_ratio) { 136 | std::lock_guard lock(mux_); 137 | damping_ratio_ = damping_ratio; 138 | _computeDamping(); 139 | } 140 | 141 | void CartesianImpedance::setNullspaceStiffness( 142 | const double &nullspace_stiffness) { 143 | std::lock_guard lock(mux_); 144 | nullspace_stiffnes_target_ = nullspace_stiffness; 145 | } 146 | 147 | void CartesianImpedance::setFilter(const double filter_coeff) { 148 | std::lock_guard lock(mux_); 149 | filter_coeff_ = filter_coeff; 150 | } 151 | 152 | void CartesianImpedance::start(const franka::RobotState &robot_state, 153 | std::shared_ptr model) { 154 | motion_finished_ = false; 155 | Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data())); 156 | Eigen::Vector3d position(transform.translation()); 157 | Eigen::Quaterniond orientation(transform.rotation()); 158 | Vector7d q = Eigen::Map(robot_state.q.data()); 159 | position_d_ = position; 160 | position_d_target_ = position; 161 | orientation_d_ = orientation; 162 | orientation_d_target_ = orientation; 163 | q_nullspace_d_ = q; 164 | q_nullspace_d_target_ = q; 165 | model_ = model; 166 | } 167 | 168 | void CartesianImpedance::stop(const franka::RobotState &robot_state, 169 | std::shared_ptr model) { 170 | motion_finished_ = true; 171 | } 172 | 173 | bool CartesianImpedance::isRunning() { return !motion_finished_; } 174 | 175 | const std::string CartesianImpedance::name() { return "Cartesian Impedance"; } -------------------------------------------------------------------------------- /src/controllers/cartesian_trajectory.cpp: -------------------------------------------------------------------------------- 1 | #include "controllers/cartesian_trajectory.h" 2 | 3 | using namespace controllers; 4 | 5 | const double CartesianTrajectory::kDefaultDqThreshold = 1e-3; 6 | const double CartesianTrajectory::kDefaultNullspaceStiffness = 15.0; 7 | // clang-format off 8 | double _data[36] = {800, 0, 0, 0, 0, 0, 9 | 0, 800, 0, 0, 0, 0, 10 | 0, 0, 800, 0, 0, 0, 11 | 0, 0, 0, 40, 0, 0, 12 | 0, 0, 0, 0, 40, 0, 13 | 0, 0, 0, 0, 0, 40}; 14 | // clang-format on 15 | const Eigen::Matrix CartesianTrajectory::kDefaultImpedance = 16 | Eigen::Matrix(_data); 17 | 18 | CartesianTrajectory::CartesianTrajectory(std::shared_ptr trajectory, 19 | const Vector7d &q_init, 20 | const Eigen::Matrix &impedance, 21 | const double &damping_ratio, 22 | const double &nullspace_stiffness, 23 | const double dq_threshold, 24 | const double filter_coeff) 25 | : CartesianImpedance(impedance, damping_ratio, nullspace_stiffness, filter_coeff), 26 | traj_(trajectory), 27 | dq_threshold_(dq_threshold), 28 | q_init_(q_init) {} 29 | 30 | franka::Torques CartesianTrajectory::step(const franka::RobotState &robot_state, 31 | franka::Duration &duration) { 32 | auto position = traj_->getPosition(getTime()); 33 | auto orientation = traj_->getOrientation(getTime()); 34 | setControl(position, orientation, q_init_); 35 | auto torques = CartesianImpedance::step(robot_state, duration); 36 | if (getTime() > traj_->getDuration()) { 37 | bool at_rest = true; 38 | for (auto dq : robot_state.dq) { 39 | if (std::abs(dq) > dq_threshold_) { 40 | at_rest = false; 41 | } 42 | } 43 | if (at_rest) { 44 | torques.motion_finished = true; 45 | } 46 | } 47 | return torques; 48 | } 49 | 50 | const std::string CartesianTrajectory::name() { 51 | return "CartesianTrajectory"; 52 | } 53 | -------------------------------------------------------------------------------- /src/controllers/force.cpp: -------------------------------------------------------------------------------- 1 | #include "controllers/force.h" 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include "panda.h" 8 | 9 | const double kDefaultDampingData[7] = {1.0, 1.0, 1.0, 1.0, 0.33, 0.33, 0.17}; 10 | const Vector7d Force::kDefaultDamping = Vector7d(kDefaultDampingData); 11 | 12 | const double Force::kDefaultFilterCoeff = 0.001; 13 | const double Force::kDefaultProportionalGain = 1; 14 | const double Force::kDefaultIntegralGain = 2; 15 | const double Force::kDefaultThreshold = 0.01; 16 | 17 | Force::Force(const double &k_p, const double &k_i, const Vector7d &damping, 18 | const double &threshold, const double &filter_coeff) 19 | : filter_coeff_(filter_coeff), 20 | k_p_(k_p), 21 | k_i_(k_i), 22 | k_p_target_(k_p), 23 | k_i_target_(k_i), 24 | K_d_(damping), 25 | K_d_target_(damping), 26 | threshold_(threshold), 27 | threshold_target_(threshold){}; 28 | 29 | franka::Torques Force::step(const franka::RobotState &robot_state, 30 | franka::Duration &duration) { 31 | // get state variables 32 | std::array jacobian_array = 33 | model_->zeroJacobian(franka::Frame::kEndEffector, robot_state); 34 | std::array gravity_array = model_->gravity(robot_state); 35 | Eigen::Map> jacobian(jacobian_array.data()); 36 | Eigen::Map tau_measured( 37 | robot_state.tau_J.data()); 38 | Eigen::Map gravity(gravity_array.data()); 39 | Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data())); 40 | Eigen::Vector3d position(transform.translation()); 41 | Eigen::Map dq(robot_state.dq.data()); 42 | 43 | // These quantities may be modified outside of the control loop 44 | mux_.lock(); 45 | _updateFilter(); 46 | Eigen::Vector3d f = f_d_; 47 | double k_p = k_p_; 48 | double k_i = k_i_; 49 | double threshold = threshold_; 50 | Vector7d K_d = K_d_; 51 | mux_.unlock(); 52 | 53 | // Abort condition 54 | if (getTime() > 0 && (position - position_init_).norm() > threshold) { 55 | throw franka::Exception(name() + ": Distance threshold exceeded."); 56 | } 57 | 58 | Eigen::VectorXd tau_d(7), force_torque_d(6), tau_ext(7); 59 | force_torque_d.setZero(); 60 | force_torque_d.head(3) = f; 61 | tau_ext << tau_measured - gravity - tau_ext_init_; 62 | tau_d << jacobian.transpose() * force_torque_d; 63 | tau_error_integral_ += duration.toSec() * (tau_d - tau_ext); 64 | // FF + PI control 65 | tau_d << tau_d + k_p * (tau_d - tau_ext) + k_i * tau_error_integral_ - K_d.asDiagonal() * dq; 66 | 67 | franka::Torques torques = VectorToArray<7>(tau_d); 68 | torques.motion_finished = motion_finished_; 69 | return torques; 70 | } 71 | 72 | void Force::_updateFilter() { 73 | f_d_ = ema_filter(f_d_, f_d_target_, filter_coeff_, true); 74 | k_p_ = ema_filter(k_p_, k_p_target_, filter_coeff_, true); 75 | k_i_ = ema_filter(k_i_, k_i_target_, filter_coeff_, true); 76 | K_d_ = ema_filter(K_d_, K_d_target_, filter_coeff_, true); 77 | threshold_ = ema_filter(threshold_, threshold_target_, filter_coeff_, true); 78 | } 79 | 80 | void Force::setControl(const Eigen::Vector3d &force) { 81 | std::lock_guard lock(mux_); 82 | f_d_target_ = force; 83 | } 84 | 85 | void Force::setFilter(const double &filter_coeff) { 86 | std::lock_guard lock(mux_); 87 | filter_coeff_ = filter_coeff; 88 | } 89 | 90 | void Force::setProportionalGain(const double &k_p) { 91 | std::lock_guard lock(mux_); 92 | k_p_target_ = k_p; 93 | } 94 | 95 | void Force::setIntegralGain(const double &k_i) { 96 | std::lock_guard lock(mux_); 97 | k_i_target_ = k_i; 98 | } 99 | 100 | void Force::setThreshold(const double &threshold) { 101 | std::lock_guard lock(mux_); 102 | threshold_target_ = threshold; 103 | } 104 | 105 | void Force::setDamping(const Vector7d &damping) { 106 | std::lock_guard lock(mux_); 107 | K_d_target_ = damping; 108 | } 109 | 110 | void Force::start(const franka::RobotState &robot_state, 111 | std::shared_ptr model) { 112 | motion_finished_ = false; 113 | f_d_.setZero(); 114 | f_d_target_.setZero(); 115 | 116 | Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data())); 117 | Eigen::Vector3d position(transform.translation()); 118 | 119 | position_init_ = position; 120 | model_ = model; 121 | 122 | // Bias torque sensor 123 | std::array gravity_array = model->gravity(robot_state); 124 | std::array tau_measured_array = robot_state.tau_J; 125 | Eigen::Map initial_tau_measured( 126 | tau_measured_array.data()); 127 | Eigen::Map initial_gravity(gravity_array.data()); 128 | tau_ext_init_ = initial_tau_measured - initial_gravity; 129 | 130 | // init integrator 131 | tau_error_integral_.setZero(); 132 | } 133 | 134 | void Force::stop(const franka::RobotState &robot_state, 135 | std::shared_ptr model) { 136 | motion_finished_ = true; 137 | } 138 | 139 | bool Force::isRunning() { return !motion_finished_; } 140 | 141 | const std::string Force::name() { return "Force Controller"; } -------------------------------------------------------------------------------- /src/controllers/integrated_velocity.cpp: -------------------------------------------------------------------------------- 1 | #include "controllers/integrated_velocity.h" 2 | #include "constants.h" 3 | #include 4 | 5 | const double kDefaultStiffnessData[7] = {600, 600, 600, 600, 250, 150, 50}; 6 | const Vector7d IntegratedVelocity::kDefaultStiffness = 7 | Vector7d(kDefaultStiffnessData); 8 | 9 | const double kDefaultDampingData[7] = {50, 50, 50, 20, 20, 20, 10}; 10 | const Vector7d IntegratedVelocity::kDefaultDamping = 11 | Vector7d(kDefaultDampingData); 12 | 13 | IntegratedVelocity::IntegratedVelocity(const Vector7d &stiffness, 14 | const Vector7d &damping) { 15 | K_p_ = stiffness; 16 | K_d_ = damping; 17 | }; 18 | 19 | franka::Torques IntegratedVelocity::step(const franka::RobotState &robot_state, 20 | franka::Duration &duration) { 21 | // Observed states 22 | Vector7d q, dq, dq_d, tau_d, K_p, K_d; 23 | q = Eigen::Map(robot_state.q.data()); 24 | dq = Eigen::Map(robot_state.dq.data()); 25 | // These quantities may be modified outside of the control loop 26 | mux_.try_lock(); 27 | dq_d = dq_d_; 28 | K_p = K_p_; 29 | K_d = K_d_; 30 | mux_.unlock(); 31 | // PD control 32 | q_d_ += duration.toSec() * dq_d; // integrate velocity 33 | q_d_ = q_d_.cwiseMin(kUpperJointLimits).cwiseMax(kLowerJointLimits); 34 | tau_d << K_p.asDiagonal() * (q_d_ - q) - K_d.asDiagonal() * dq; 35 | franka::Torques torques = VectorToArray(tau_d); 36 | torques.motion_finished = motion_finished_; 37 | return torques; 38 | } 39 | 40 | Vector7d IntegratedVelocity::getQd() { 41 | return q_d_; 42 | } 43 | 44 | void IntegratedVelocity::setControl(const Vector7d &velocity) { 45 | std::lock_guard lock(mux_); 46 | dq_d_ = velocity; 47 | } 48 | 49 | void IntegratedVelocity::setStiffness(const Vector7d &stiffness) { 50 | std::lock_guard lock(mux_); 51 | K_p_ = stiffness; 52 | } 53 | 54 | void IntegratedVelocity::setDamping(const Vector7d &damping) { 55 | std::lock_guard lock(mux_); 56 | K_d_ = damping; 57 | } 58 | 59 | void IntegratedVelocity::start(const franka::RobotState &robot_state, std::shared_ptr model) { 60 | motion_finished_ = false; 61 | q_d_ = Eigen::Map(robot_state.q.data()); 62 | dq_d_.setZero(); 63 | } 64 | 65 | void IntegratedVelocity::stop(const franka::RobotState &robot_state, std::shared_ptr model) { 66 | motion_finished_ = true; 67 | } 68 | 69 | bool IntegratedVelocity::isRunning() { 70 | return !motion_finished_; 71 | } 72 | 73 | const std::string IntegratedVelocity::name() { 74 | return "Integrated Velocity"; 75 | } -------------------------------------------------------------------------------- /src/controllers/joint_limits/virtual_wall.cpp: -------------------------------------------------------------------------------- 1 | #include "controllers/joint_limits/virtual_wall.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | using controllers::joint_limits::VirtualWall; 8 | 9 | VirtualWall::VirtualWall(const double& soft_upper_joint_position_limit, 10 | const double& soft_lower_joint_position_limit, 11 | const double& PD_zone_width, const double& D_zone_width, 12 | const double& PD_zone_stiffness, 13 | const double& PD_zone_damping, 14 | const double& D_zone_damping) 15 | : soft_upper_joint_position_limit_(soft_upper_joint_position_limit), 16 | soft_lower_joint_position_limit_(soft_lower_joint_position_limit), 17 | PD_zone_width_(PD_zone_width), 18 | D_zone_width_(D_zone_width), 19 | PD_zone_stiffness_(PD_zone_stiffness), 20 | PD_zone_damping_(PD_zone_damping), 21 | D_zone_damping_(D_zone_damping) { 22 | PD_zone_width_ = positiveCheck(PD_zone_width); 23 | D_zone_width_ = positiveCheck(D_zone_width); 24 | PD_zone_stiffness_ = positiveCheck(PD_zone_stiffness); 25 | PD_zone_damping_ = positiveCheck(PD_zone_damping); 26 | D_zone_damping_ = positiveCheck(D_zone_damping); 27 | }; 28 | 29 | double VirtualWall::computeTorque(const double& q, const double& dq) { 30 | init(q, dq); 31 | adjustMovingWall(q, dq); 32 | 33 | double D_zone_boundary_max = 34 | soft_upper_joint_position_limit_ - 35 | zone_width_scale_ * (PD_zone_width_ + D_zone_width_); 36 | double PD_zone_boundary_max = 37 | soft_upper_joint_position_limit_ - zone_width_scale_ * PD_zone_width_; 38 | double D_zone_boundary_min = 39 | soft_lower_joint_position_limit_ + 40 | zone_width_scale_ * (PD_zone_width_ + D_zone_width_); 41 | double PD_zone_boundary_min = 42 | soft_lower_joint_position_limit_ + zone_width_scale_ * PD_zone_width_; 43 | 44 | double torque = 0; 45 | // In D zone 46 | if (inRange(D_zone_boundary_max, PD_zone_boundary_max, q) || 47 | inRange(PD_zone_boundary_min, D_zone_boundary_min, q)) { 48 | torque = -D_zone_damping_ * dq; 49 | // PD zone max 50 | } else if (q > PD_zone_boundary_max) { 51 | torque = -PD_zone_damping_ * dq + 52 | PD_zone_stiffness_ * (PD_zone_boundary_max - q); 53 | // PD zone min 54 | } else if (q < PD_zone_boundary_min) { 55 | torque = -PD_zone_damping_ * dq + 56 | PD_zone_stiffness_ * (PD_zone_boundary_min - q); 57 | } 58 | 59 | return torque; 60 | } 61 | 62 | void VirtualWall::reset() { initialized_ = false; } 63 | 64 | bool VirtualWall::inRange(double low, double high, double x) { 65 | return (low <= x && x <= high); 66 | }; 67 | 68 | void VirtualWall::init(const double& q, const double& dq) { 69 | if (initialized_) { 70 | return; 71 | } 72 | 73 | if (q < soft_lower_joint_position_limit_ || 74 | q > soft_upper_joint_position_limit_) { 75 | std::stringstream ss; 76 | ss << "q " << q << " is beyond the joint wall: [" 77 | << soft_lower_joint_position_limit_ << ", " 78 | << soft_upper_joint_position_limit_ << "]"; 79 | throw std::runtime_error(ss.str().c_str()); 80 | } 81 | 82 | switch (getMotionInWall(q, dq)) { 83 | case MotionInWall::EnteringNormal: 84 | moving_wall_ = false; 85 | break; 86 | case MotionInWall::PenetratingLowerLimit: 87 | case MotionInWall::LeavingLowerLimit: 88 | zone_width_scale_ = fabs(q - soft_lower_joint_position_limit_) / 89 | (PD_zone_width_ + D_zone_width_); 90 | moving_wall_ = true; 91 | break; 92 | case MotionInWall::PenetratingUpperLimit: 93 | case MotionInWall::LeavingUpperLimit: 94 | zone_width_scale_ = fabs(q - soft_upper_joint_position_limit_) / 95 | (PD_zone_width_ + D_zone_width_); 96 | moving_wall_ = true; 97 | break; 98 | } 99 | initialized_ = true; 100 | } 101 | 102 | void VirtualWall::adjustMovingWall(const double& q, const double& dq) { 103 | if (!moving_wall_) { 104 | return; 105 | } 106 | double new_scale; 107 | switch (getMotionInWall(q, dq)) { 108 | case MotionInWall::EnteringNormal: 109 | moving_wall_ = false; 110 | zone_width_scale_ = 1; 111 | break; 112 | case MotionInWall::LeavingLowerLimit: 113 | new_scale = fabs(q - soft_lower_joint_position_limit_) / 114 | (PD_zone_width_ + D_zone_width_); 115 | zone_width_scale_ = fmax(zone_width_scale_, new_scale); 116 | break; 117 | case MotionInWall::LeavingUpperLimit: 118 | new_scale = fabs(q - soft_upper_joint_position_limit_) / 119 | (PD_zone_width_ + D_zone_width_); 120 | zone_width_scale_ = fmax(zone_width_scale_, new_scale); 121 | break; 122 | case MotionInWall::PenetratingLowerLimit: 123 | case MotionInWall::PenetratingUpperLimit: 124 | break; 125 | } 126 | } 127 | 128 | double VirtualWall::positiveCheck(double value) { 129 | if (value < 0) { 130 | printf( 131 | "ERROR: VirtualWall expects positive parameters, but got negative. Using " 132 | "its absolute " 133 | "value."); 134 | value = fabs(value); 135 | } 136 | return value; 137 | } 138 | 139 | VirtualWall::MotionInWall VirtualWall::getMotionInWall(const double& q, 140 | const double& dq) const { 141 | double D_zone_boundary_max = 142 | soft_upper_joint_position_limit_ - PD_zone_width_ - D_zone_width_; 143 | double D_zone_boundary_min = 144 | soft_lower_joint_position_limit_ + PD_zone_width_ + D_zone_width_; 145 | if (q < D_zone_boundary_min) { 146 | if (dq <= 0) { 147 | return MotionInWall::PenetratingLowerLimit; 148 | } 149 | return MotionInWall::LeavingLowerLimit; 150 | } 151 | if (q > D_zone_boundary_max) { 152 | if (dq >= 0) { 153 | return MotionInWall::PenetratingUpperLimit; 154 | } 155 | return MotionInWall::LeavingUpperLimit; 156 | } 157 | return MotionInWall::EnteringNormal; 158 | } 159 | -------------------------------------------------------------------------------- /src/controllers/joint_position.cpp: -------------------------------------------------------------------------------- 1 | #include "controllers/joint_position.h" 2 | 3 | #include 4 | 5 | #include "panda.h" 6 | 7 | const double kDefaultStiffnessData[7] = {600, 600, 600, 600, 250, 150, 50}; 8 | const Vector7d JointPosition::kDefaultStiffness = 9 | Vector7d(kDefaultStiffnessData); 10 | 11 | const double kDefaultDqdData[7] = {0, 0, 0, 0, 0, 0, 0}; 12 | const Vector7d JointPosition::kDefaultDqd = Vector7d(kDefaultDqdData); 13 | 14 | const double kDefaultDampingData[7] = {50, 50, 50, 20, 20, 20, 10}; 15 | const Vector7d JointPosition::kDefaultDamping = Vector7d(kDefaultDampingData); 16 | const double JointPosition::kDefaultFilterCoeff = 1.0; 17 | 18 | JointPosition::JointPosition(const Vector7d &stiffness, const Vector7d &damping, 19 | const double filter_coeff) { 20 | K_p_ = stiffness; 21 | K_p_target_ = stiffness; 22 | K_d_ = damping; 23 | K_d_target_ = damping; 24 | filter_coeff_ = filter_coeff; 25 | }; 26 | 27 | franka::Torques JointPosition::step(const franka::RobotState &robot_state, 28 | franka::Duration &duration) { 29 | // Observed states 30 | Vector7d q, q_d, dq_d, dq, tau_d, K_p, K_d; 31 | q = Eigen::Map(robot_state.q.data()); 32 | dq = Eigen::Map(robot_state.dq.data()); 33 | // These quantities may be modified outside of the control loop 34 | mux_.lock(); 35 | _updateFilter(); 36 | K_p = K_p_; 37 | K_d = K_d_; 38 | q_d = q_d_; 39 | dq_d = dq_d_; 40 | mux_.unlock(); 41 | // PD control 42 | tau_d << K_p.asDiagonal() * (q_d - q) + K_d.asDiagonal() * (dq_d - dq); 43 | franka::Torques torques = VectorToArray(tau_d); 44 | torques.motion_finished = motion_finished_; 45 | return torques; 46 | } 47 | 48 | void JointPosition::_updateFilter() { 49 | q_d_ = ema_filter(q_d_, q_d_target_, filter_coeff_, true); 50 | dq_d_ = ema_filter(dq_d_, dq_d_target_, filter_coeff_, true); 51 | K_p_ = ema_filter(K_p_, K_p_target_, filter_coeff_, true); 52 | K_d_ = ema_filter(K_d_, K_d_target_, filter_coeff_, true); 53 | } 54 | 55 | void JointPosition::setControl(const Vector7d &position, 56 | const Vector7d &velocity) { 57 | std::lock_guard lock(mux_); 58 | q_d_target_ = position; 59 | dq_d_target_ = velocity; 60 | } 61 | 62 | void JointPosition::setStiffness(const Vector7d &stiffness) { 63 | std::lock_guard lock(mux_); 64 | K_p_target_ = stiffness; 65 | } 66 | 67 | void JointPosition::setDamping(const Vector7d &damping) { 68 | std::lock_guard lock(mux_); 69 | K_d_target_ = damping; 70 | } 71 | 72 | void JointPosition::setFilter(const double filter_coeff) { 73 | std::lock_guard lock(mux_); 74 | filter_coeff_ = filter_coeff; 75 | } 76 | 77 | void JointPosition::start(const franka::RobotState &robot_state, 78 | std::shared_ptr model) { 79 | motion_finished_ = false; 80 | q_d_ = Eigen::Map(robot_state.q.data()); 81 | q_d_target_ = Eigen::Map(robot_state.q.data()); 82 | dq_d_.setZero(); 83 | dq_d_target_.setZero(); 84 | } 85 | 86 | void JointPosition::stop(const franka::RobotState &robot_state, 87 | std::shared_ptr model) { 88 | motion_finished_ = true; 89 | } 90 | 91 | bool JointPosition::isRunning() { return !motion_finished_; } 92 | 93 | const std::string JointPosition::name() { return "Joint Position"; } -------------------------------------------------------------------------------- /src/controllers/joint_trajectory.cpp: -------------------------------------------------------------------------------- 1 | #include "controllers/joint_trajectory.h" 2 | 3 | using namespace controllers; 4 | 5 | const double JointTrajectory::kDefaultDqThreshold = 1e-3; 6 | 7 | JointTrajectory::JointTrajectory(std::shared_ptr trajectory, 8 | const Vector7d &stiffness, const Vector7d &damping, 9 | const double dq_threshold, const double filter_coeff) 10 | : JointPosition(stiffness, damping, filter_coeff), 11 | traj_(trajectory), 12 | dq_threshold_(dq_threshold) {} 13 | 14 | franka::Torques JointTrajectory::step(const franka::RobotState &robot_state, 15 | franka::Duration &duration) { 16 | auto q_d = traj_->getJointPositions(getTime()); 17 | auto dq_d = traj_->getJointVelocities(getTime()); 18 | setControl(q_d, dq_d); 19 | auto torques = JointPosition::step(robot_state, duration); 20 | if (getTime() > traj_->getDuration()) { 21 | bool at_rest = true; 22 | for (auto dq : robot_state.dq) { 23 | if (std::abs(dq) > dq_threshold_) { 24 | at_rest = false; 25 | } 26 | } 27 | if (at_rest) { 28 | torques.motion_finished = true; 29 | } 30 | } 31 | return torques; 32 | } 33 | 34 | const std::string JointTrajectory::name() { 35 | return "JointTrajectory"; 36 | } -------------------------------------------------------------------------------- /src/motion/generators.cpp: -------------------------------------------------------------------------------- 1 | #include "motion/generators.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "constants.h" 8 | 9 | using namespace std; 10 | using namespace Eigen; 11 | using namespace motion; 12 | 13 | bool PandaTrajectory::_computeTrajectory( 14 | const time_optimal::Path &path, const Eigen::VectorXd &max_velocity, 15 | const Eigen::VectorXd &max_acceleration, double timeout) { 16 | auto startTime = std::chrono::high_resolution_clock::now(); 17 | bool success = false; 18 | int i = 0; 19 | while (true) { 20 | i++; 21 | traj_ = std::make_shared(path, max_velocity, 22 | max_acceleration, 1e-3); 23 | if (traj_->isValid() && !isnan(traj_->getDuration())) { 24 | success = true; 25 | break; 26 | } 27 | auto currentTime = std::chrono::high_resolution_clock::now(); 28 | auto duration = std::chrono::duration_cast( 29 | currentTime - startTime) 30 | .count(); 31 | if (duration >= timeout) { 32 | _log("error", "Trajectory computation timed out after %d seconds.", 33 | duration); 34 | break; 35 | } 36 | _log("debug", "Reattempting trajectory computation. Attempt no. %d.", i); 37 | } 38 | return success; 39 | } 40 | 41 | JointTrajectory::JointTrajectory(const std::vector &waypoints, 42 | double speed_factor, double maxDeviation, 43 | double timeout) { 44 | py::gil_scoped_acquire acquire; 45 | py::object logging = py::module_::import("logging"); 46 | logger_ = logging.attr("getLogger")("motion"); 47 | py::gil_scoped_release release; 48 | 49 | if (!_computeTrajectory(_convertList(waypoints, maxDeviation), 50 | speed_factor * kQMaxVelocity, 51 | speed_factor * kQMaxAcceleration, timeout)) { 52 | throw runtime_error("Trajectory generation faild."); 53 | } 54 | 55 | if (waypoints.size() == 2) { 56 | _log("info", 57 | "Computed joint trajectory: 1 waypoint, duration %.2f seconds.", 58 | traj_->getDuration()); 59 | } else { 60 | _log("info", 61 | "Computed joint trajectory: %d waypoints, duration %.2f seconds.", 62 | waypoints.size() - 1, traj_->getDuration()); 63 | } 64 | } 65 | 66 | time_optimal::Path JointTrajectory::_convertList( 67 | const std::vector &list, double maxDeviation) { 68 | std::list new_list; 69 | for (auto l : list) { 70 | new_list.push_back(Eigen::Map(l.data(), 7, 1)); 71 | } 72 | return time_optimal::Path(new_list, maxDeviation); 73 | } 74 | 75 | Vector7d JointTrajectory::getJointPositions(double time) { 76 | return traj_->getPosition(time); 77 | } 78 | 79 | Vector7d JointTrajectory::getJointVelocities(double time) { 80 | return traj_->getVelocity(time); 81 | } 82 | 83 | Vector7d JointTrajectory::getJointAccelerations(double time) { 84 | return traj_->getAcceleration(time); 85 | } 86 | 87 | CartesianTrajectory::CartesianTrajectory( 88 | const std::vector> &poses, double speed_factor, 89 | double maxDeviation, double timeout) { 90 | std::vector> positions; 91 | std::vector> orientations; 92 | for (auto p : poses) { 93 | positions.push_back(MatrixToPosition(p)); 94 | orientations.push_back(MatrixToOrientation(p)); 95 | } 96 | CartesianTrajectory(positions, orientations, speed_factor, maxDeviation, 97 | timeout); 98 | } 99 | 100 | CartesianTrajectory::CartesianTrajectory( 101 | const std::vector> &positions, 102 | const std::vector> &orientations, 103 | double speed_factor, double maxDeviation, double timeout) { 104 | py::gil_scoped_acquire acquire; 105 | py::object logging = py::module_::import("logging"); 106 | logger_ = logging.attr("getLogger")("motion"); 107 | py::gil_scoped_release release; 108 | angles_.push_back(0); 109 | 110 | for (size_t i = 0; i < orientations.size() - 1; i++) { 111 | Eigen::Quaterniond q1(orientations.at(i)), q2(orientations.at(i + 1)); 112 | Eigen::AngleAxisd aa(q2 * q1.inverse()); 113 | double angle = aa.angle(); 114 | Eigen::Vector3d axis = aa.axis(); 115 | angles_.push_back(angle); 116 | axes_.push_back(axis); 117 | orientations_.push_back(q1); 118 | } 119 | orientations_.push_back(Eigen::Quaterniond(orientations.back())); 120 | std::partial_sum(angles_.begin(), angles_.end(), angles_.begin()); 121 | std::list waypoints; 122 | 123 | for (size_t i = 0; i < orientations.size(); i++) { 124 | Eigen::VectorXd point; 125 | point.resize(4); 126 | point.head(3) = positions.at(i); 127 | point.coeffRef(3) = angles_.at(i); 128 | waypoints.push_back(point); 129 | } 130 | 131 | if (!_computeTrajectory(time_optimal::Path(waypoints, maxDeviation), 132 | speed_factor * kXMaxVelocity, 133 | speed_factor * kXMaxAcceleration, timeout)) { 134 | throw runtime_error("Trajectory generation failed."); 135 | } 136 | 137 | if (orientations.size() == 2) { 138 | _log("info", 139 | "Computed Cartesian trajectory: 1 waypoint, duration %.2f seconds.", 140 | traj_->getDuration()); 141 | } else { 142 | _log("info", 143 | "Computed Cartesian trajectory: %d waypoints, duration %.2f seconds.", 144 | orientations.size() - 1, traj_->getDuration()); 145 | } 146 | } 147 | 148 | Eigen::Matrix CartesianTrajectory::getPose(double time) { 149 | auto pose = traj_->getPosition(time); 150 | size_t idx = traj_->getTrajectorySegmentIndex(time); 151 | double angle = pose.coeff(3) - angles_.at(idx); 152 | Eigen::AngleAxisd aa(angle, axes_.at(idx)); 153 | Eigen::Quaterniond o = Eigen::Quaterniond(aa) * orientations_.at(idx); 154 | 155 | Eigen::Affine3d transform; 156 | transform = o; 157 | transform.translation() = pose.head(3); 158 | 159 | return transform.matrix(); 160 | } 161 | 162 | Eigen::Vector3d CartesianTrajectory::getPosition(double time) { 163 | auto pose = traj_->getPosition(time); 164 | return pose.head(3); 165 | } 166 | 167 | Eigen::Vector4d CartesianTrajectory::getOrientation(double time) { 168 | auto pose = traj_->getPosition(time); 169 | size_t idx = traj_->getTrajectorySegmentIndex(time); 170 | double angle = pose.coeff(3) - angles_.at(idx); 171 | Eigen::AngleAxisd aa(angle, axes_.at(idx)); 172 | Eigen::Quaterniond o = Eigen::Quaterniond(aa) * orientations_.at(idx); 173 | return o.coeffs(); 174 | } 175 | -------------------------------------------------------------------------------- /src/motion/time_optimal/path.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2011, Georgia Tech Research Corporation 3 | * All rights reserved. 4 | * 5 | * Author: Tobias Kunz 6 | * Date: 05/2012 7 | * 8 | * Humanoid Robotics Lab Georgia Institute of Technology 9 | * Director: Mike Stilman http://www.golems.org 10 | * 11 | * Algorithm details and publications: 12 | * http://www.golems.org/node/1570 13 | * 14 | * This file is provided under the following "BSD-style" License: 15 | * Redistribution and use in source and binary forms, with or 16 | * without modification, are permitted provided that the following 17 | * conditions are met: 18 | * * Redistributions of source code must retain the above copyright 19 | * notice, this list of conditions and the following disclaimer. 20 | * * Redistributions in binary form must reproduce the above 21 | * copyright notice, this list of conditions and the following 22 | * disclaimer in the documentation and/or other materials provided 23 | * with the distribution. 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 25 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 26 | * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 27 | * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 29 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 30 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 31 | * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF 32 | * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 33 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 34 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 35 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 36 | * POSSIBILITY OF SUCH DAMAGE. 37 | */ 38 | 39 | #include "motion/time_optimal/path.h" 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | using namespace motion::time_optimal; 51 | 52 | class LinearPathSegment : public PathSegment { 53 | public: 54 | LinearPathSegment(const Eigen::VectorXd& start, const Eigen::VectorXd& end) 55 | : PathSegment((end - start).norm()), end_(end), start_(start) {} 56 | 57 | Eigen::VectorXd getConfig(double s) const override { 58 | s /= length_; 59 | s = std::max(0.0, std::min(1.0, s)); 60 | return (1.0 - s) * start_ + s * end_; 61 | } 62 | 63 | Eigen::VectorXd getTangent(double /* s */) const override { 64 | return (end_ - start_) / length_; 65 | } 66 | 67 | Eigen::VectorXd getCurvature(double /* s */) const override { 68 | return Eigen::VectorXd::Zero(start_.size()); 69 | } 70 | 71 | std::list getSwitchingPoints() const override { 72 | return std::list(); 73 | } 74 | 75 | LinearPathSegment* clone() const override { 76 | return new LinearPathSegment(*this); 77 | } 78 | 79 | private: 80 | Eigen::VectorXd end_; 81 | Eigen::VectorXd start_; 82 | }; 83 | 84 | class CircularPathSegment : public PathSegment { 85 | public: 86 | CircularPathSegment(const Eigen::VectorXd& start, 87 | const Eigen::VectorXd& intersection, 88 | const Eigen::VectorXd& end, double max_deviation) { 89 | if ((intersection - start).norm() < 0.000001 || 90 | (end - intersection).norm() < 0.000001) { 91 | length_ = 0.0; 92 | radius = 1.0; 93 | center = intersection; 94 | x = Eigen::VectorXd::Zero(start.size()); 95 | y = Eigen::VectorXd::Zero(start.size()); 96 | return; 97 | } 98 | 99 | const Eigen::VectorXd start_direction = (intersection - start).normalized(); 100 | const Eigen::VectorXd end_direction = (end - intersection).normalized(); 101 | const double start_dot_end = start_direction.dot(end_direction); 102 | 103 | // catch division by 0 in computations below 104 | if (start_dot_end > 0.999999 || start_dot_end < -0.999999) { 105 | length_ = 0.0; 106 | radius = 1.0; 107 | center = intersection; 108 | x = Eigen::VectorXd::Zero(start.size()); 109 | y = Eigen::VectorXd::Zero(start.size()); 110 | return; 111 | } 112 | 113 | const double angle = acos(start_dot_end); 114 | const double start_distance = (start - intersection).norm(); 115 | const double end_distance = (end - intersection).norm(); 116 | 117 | // enforce max deviation 118 | double distance = std::min(start_distance, end_distance); 119 | distance = std::min( 120 | distance, max_deviation * sin(0.5 * angle) / (1.0 - cos(0.5 * angle))); 121 | 122 | radius = distance / tan(0.5 * angle); 123 | length_ = angle * radius; 124 | 125 | center = intersection + (end_direction - start_direction).normalized() * 126 | radius / cos(0.5 * angle); 127 | x = (intersection - distance * start_direction - center).normalized(); 128 | y = start_direction; 129 | } 130 | 131 | Eigen::VectorXd getConfig(double s) const override { 132 | const double angle = s / radius; 133 | return center + radius * (x * cos(angle) + y * sin(angle)); 134 | } 135 | 136 | Eigen::VectorXd getTangent(double s) const override { 137 | const double angle = s / radius; 138 | return -x * sin(angle) + y * cos(angle); 139 | } 140 | 141 | Eigen::VectorXd getCurvature(double s) const override { 142 | const double angle = s / radius; 143 | return -1.0 / radius * (x * cos(angle) + y * sin(angle)); 144 | } 145 | 146 | std::list getSwitchingPoints() const override { 147 | std::list switching_points; 148 | const double dim = x.size(); 149 | for (unsigned int i = 0; i < dim; ++i) { 150 | double switching_angle = atan2(y[i], x[i]); 151 | if (switching_angle < 0.0) { 152 | switching_angle += M_PI; 153 | } 154 | const double switching_point = switching_angle * radius; 155 | if (switching_point < length_) { 156 | switching_points.push_back(switching_point); 157 | } 158 | } 159 | switching_points.sort(); 160 | return switching_points; 161 | } 162 | 163 | CircularPathSegment* clone() const override { 164 | return new CircularPathSegment(*this); 165 | } 166 | 167 | private: 168 | double radius; 169 | Eigen::VectorXd center; 170 | Eigen::VectorXd x; 171 | Eigen::VectorXd y; 172 | }; 173 | 174 | Path::Path(const std::list& path, double max_deviation) 175 | : length_(0.0) { 176 | if (path.size() < 2) return; 177 | std::list::const_iterator path_iterator1 = path.begin(); 178 | std::list::const_iterator path_iterator2 = path_iterator1; 179 | ++path_iterator2; 180 | std::list::const_iterator path_iterator3; 181 | Eigen::VectorXd start_config = *path_iterator1; 182 | while (path_iterator2 != path.end()) { 183 | double section_length = 0.0; 184 | path_iterator3 = path_iterator2; 185 | ++path_iterator3; 186 | if (max_deviation > 0.0 && path_iterator3 != path.end()) { 187 | CircularPathSegment* blend_segment = new CircularPathSegment( 188 | 0.5 * (*path_iterator1 + *path_iterator2), *path_iterator2, 189 | 0.5 * (*path_iterator2 + *path_iterator3), max_deviation); 190 | Eigen::VectorXd end_config = blend_segment->getConfig(0.0); 191 | if ((end_config - start_config).norm() > 0.000001) { 192 | path_segments_.push_back( 193 | std::make_unique(start_config, end_config)); 194 | section_length += path_segments_.back()->getLength(); 195 | } 196 | path_segments_.emplace_back(blend_segment); 197 | section_length += path_segments_.back()->getLength()/2.0; 198 | start_config = blend_segment->getConfig(blend_segment->getLength()); 199 | } else { 200 | path_segments_.push_back( 201 | std::make_unique(start_config, *path_iterator2)); 202 | start_config = *path_iterator2; 203 | section_length = path_segments_.back()->getLength(); 204 | } 205 | path_iterator1 = path_iterator2; 206 | ++path_iterator2; 207 | section_lengths.push_back(section_length); 208 | } 209 | std::partial_sum(section_lengths.begin(), section_lengths.end(), section_lengths.begin()); 210 | 211 | // Create list of switching point candidates, calculate total path length and 212 | // absolute positions of path segments 213 | for (std::unique_ptr& path_segment : path_segments_) { 214 | path_segment->position_ = length_; 215 | std::list local_switching_points = 216 | path_segment->getSwitchingPoints(); 217 | for (std::list::const_iterator point = 218 | local_switching_points.begin(); 219 | point != local_switching_points.end(); ++point) { 220 | switching_points_.push_back(std::make_pair(length_ + *point, false)); 221 | } 222 | length_ += path_segment->getLength(); 223 | while (!switching_points_.empty() && 224 | switching_points_.back().first >= length_) 225 | switching_points_.pop_back(); 226 | switching_points_.push_back(std::make_pair(length_, true)); 227 | } 228 | switching_points_.pop_back(); 229 | } 230 | 231 | Path::Path(const Path& path) 232 | : length_(path.length_), switching_points_(path.switching_points_) { 233 | for (const std::unique_ptr& path_segment : path.path_segments_) { 234 | path_segments_.emplace_back(path_segment->clone()); 235 | } 236 | section_lengths = path.section_lengths; 237 | } 238 | 239 | double Path::getLength() const { return length_; } 240 | 241 | PathSegment* Path::getPathSegment(double& s) const { 242 | std::list>::const_iterator it = 243 | path_segments_.begin(); 244 | std::list>::const_iterator next = it; 245 | ++next; 246 | while (next != path_segments_.end() && s >= (*next)->position_) { 247 | it = next; 248 | ++next; 249 | } 250 | s -= (*it)->position_; 251 | return (*it).get(); 252 | } 253 | 254 | Eigen::VectorXd Path::getConfig(double s) const { 255 | const PathSegment* path_segment = getPathSegment(s); 256 | return path_segment->getConfig(s); 257 | } 258 | 259 | Eigen::VectorXd Path::getTangent(double s) const { 260 | const PathSegment* path_segment = getPathSegment(s); 261 | return path_segment->getTangent(s); 262 | } 263 | 264 | Eigen::VectorXd Path::getCurvature(double s) const { 265 | const PathSegment* path_segment = getPathSegment(s); 266 | return path_segment->getCurvature(s); 267 | } 268 | 269 | double Path::getNextSwitchingPoint(double s, bool& discontinuity) const { 270 | std::list>::const_iterator it = 271 | switching_points_.begin(); 272 | while (it != switching_points_.end() && it->first <= s) { 273 | ++it; 274 | } 275 | if (it == switching_points_.end()) { 276 | discontinuity = true; 277 | return length_; 278 | } 279 | discontinuity = it->second; 280 | return it->first; 281 | } 282 | 283 | std::list> Path::getSwitchingPoints() const { 284 | return switching_points_; 285 | } -------------------------------------------------------------------------------- /src/panda_py/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | Introduction 3 | ------------ 4 | 5 | panda-py is a Python library for the Franka Emika Robot System 6 | that allows you to program and control the robot in real-time. 7 | 8 | 9 | """ 10 | 11 | import base64 12 | import configparser 13 | import dataclasses 14 | import hashlib 15 | import json as json_module 16 | import logging 17 | import os 18 | import ssl 19 | import threading 20 | import typing 21 | from urllib import parse 22 | 23 | import requests 24 | from requests.packages import urllib3 25 | from websockets.sync.client import connect 26 | 27 | # pylint: disable=no-name-in-module 28 | from ._core import Panda, PandaContext, fk, ik, ik_full 29 | 30 | __all__ = [ 31 | 'Panda', 'PandaContext', 'constants', 'controllers', 'libfranka', 'motion', 32 | 'fk', 'ik', 'ik_full', 'Desk', 'TOKEN_PATH' 33 | ] 34 | 35 | __version__ = '0.8.1' 36 | 37 | _logger = logging.getLogger('desk') 38 | 39 | TOKEN_PATH = '~/.panda_py/token.conf' 40 | """ 41 | Path to the configuration file holding known control tokens. 42 | If :py:class:`Desk` is used to connect to a control unit's 43 | web interface and takes control, the generated token is stored 44 | in this file under the unit's IP address or hostname. 45 | """ 46 | 47 | 48 | @dataclasses.dataclass 49 | class Token: 50 | """ 51 | Represents a Desk token owned by a user. 52 | """ 53 | id: str = '' 54 | owned_by: str = '' 55 | token: str = '' 56 | 57 | 58 | class Desk: 59 | """ 60 | Connects to the control unit running the web-based Desk interface 61 | to manage the robot. Use this class to interact with the Desk 62 | from Python, e.g. if you use a headless setup. This interface 63 | supports common tasks such as unlocking the brakes, activating 64 | the FCI etc. 65 | 66 | Newer versions of the system software use role-based access 67 | management to allow only one user to be in control of the Desk 68 | at a time. The controlling user is authenticated using a token. 69 | The :py:class:`Desk` class saves those token in :py:obj:`TOKEN_PATH` 70 | and will use them when reconnecting to the Desk, retaking control. 71 | Without a token, control of a Desk can only be taken, if there is 72 | no active claim or the controlling user explicitly relinquishes control. 73 | If the controlling user's token is lost, a user can take control 74 | forcefully (cf. :py:func:`Desk.take_control`) but needs to confirm 75 | physical access to the robot by pressing the circle button on the 76 | robot's Pilot interface. 77 | """ 78 | 79 | def __init__(self, 80 | hostname: str, 81 | username: str, 82 | password: str, 83 | platform: str = 'panda') -> None: 84 | urllib3.disable_warnings() 85 | self._session = requests.Session() 86 | self._session.verify = False 87 | self._hostname = hostname 88 | self._username = username 89 | self._password = password 90 | self._logged_in = False 91 | self._token = self._load_token() 92 | self._listening = False 93 | self._listen_thread = None 94 | self.login() 95 | self._legacy = False 96 | 97 | if platform.lower() in [ 98 | 'panda', 'fer', 'franka_emika_robot', 'frankaemikarobot' 99 | ]: 100 | self._platform = 'panda' 101 | elif platform.lower() in ['fr3', 'frankaresearch3', 'franka_research_3']: 102 | self._platform = 'fr3' 103 | else: 104 | raise ValueError("Unknown platform! Must be either 'panda' or 'fr3'!") 105 | 106 | try: 107 | self.take_control() 108 | except ConnectionError as error: 109 | if 'File not found' in str(error): 110 | _logger.info('Legacy desk detected.') 111 | self._legacy = True 112 | else: 113 | raise error 114 | 115 | def lock(self, force: bool = True) -> None: 116 | """ 117 | Locks the brakes. API call blocks until the brakes are locked. 118 | """ 119 | if self._platform == 'panda': 120 | url = '/desk/api/robot/close-brakes' 121 | elif self._platform == 'fr3': 122 | url = '/desk/api/joints/lock' 123 | 124 | self._request('post', url, files={'force': force}) 125 | 126 | def unlock(self, force: bool = True) -> None: 127 | """ 128 | Unlocks the brakes. API call blocks until the brakes are unlocked. 129 | """ 130 | if self._platform == 'panda': 131 | url = '/desk/api/robot/open-brakes' 132 | elif self._platform == 'fr3': 133 | url = '/desk/api/joints/unlock' 134 | 135 | self._request('post', 136 | url, 137 | files={'force': force}, 138 | headers={'X-Control-Token': self._token.token}) 139 | 140 | def reboot(self) -> None: 141 | """ 142 | Reboots the robot hardware (this will close open connections). 143 | """ 144 | self._request('post', 145 | '/admin/api/reboot', 146 | headers={'X-Control-Token': self._token.token}) 147 | 148 | def activate_fci(self) -> None: 149 | """ 150 | Activates the Franka Research Interface (FCI). Note that the 151 | brakes must be unlocked first. For older Desk versions, this 152 | function does nothing. 153 | """ 154 | if not self._legacy: 155 | self._request('post', 156 | '/admin/api/control-token/fci', 157 | json={'token': self._token.token}) 158 | 159 | def deactivate_fci(self) -> None: 160 | """ 161 | Deactivates the Franka Research Interface (FCI). For older 162 | Desk versions, this function does nothing. 163 | """ 164 | if not self._legacy: 165 | self._request('delete', 166 | '/admin/api/control-token/fci', 167 | json={'token': self._token.token}) 168 | 169 | def _load_token(self) -> Token: 170 | config_path = os.path.expanduser(TOKEN_PATH) 171 | config = configparser.ConfigParser() 172 | token = Token() 173 | if os.path.exists(config_path): 174 | config.read(config_path) 175 | if config.has_section(self._hostname): 176 | token.id = config.get(self._hostname, 'id') 177 | token.owned_by = config.get(self._hostname, 'owned_by') 178 | token.token = config.get(self._hostname, 'token') 179 | return token 180 | 181 | def _save_token(self, token: Token) -> None: 182 | config_path = os.path.expanduser(TOKEN_PATH) 183 | config = configparser.ConfigParser() 184 | if os.path.exists(config_path): 185 | config.read(config_path) 186 | config[self._hostname] = { 187 | 'id': token.id, 188 | 'owned_by': token.owned_by, 189 | 'token': token.token 190 | } 191 | os.makedirs(os.path.dirname(config_path), exist_ok=True) 192 | with open(config_path, 'w') as config_file: 193 | config.write(config_file) 194 | self._token = token 195 | 196 | def take_control(self, force: bool = False) -> bool: 197 | """ 198 | Takes control of the Desk, generating a new control token and saving it. 199 | If `force` is set to True, control can be taken forcefully even if another 200 | user is already in control. However, the user will have to press the circle 201 | button on the robot's Pilot within an alotted amount of time to confirm 202 | physical access. 203 | 204 | For legacy versions of the Desk, this function does nothing. 205 | """ 206 | if self._legacy: 207 | return True 208 | active = self._get_active_token() 209 | if active.id != '' and self._token.id == active.id: 210 | _logger.info('Retaken control.') 211 | return True 212 | if active.id != '' and not force: 213 | _logger.warning('Cannot take control. User %s is in control.', 214 | active.owned_by) 215 | return False 216 | response = self._request( 217 | 'post', 218 | f'/admin/api/control-token/request{"?force" if force else ""}', 219 | json={ 220 | 'requestedBy': self._username 221 | }).json() 222 | if force: 223 | timeout = self._request('get', 224 | '/admin/api/safety').json()['tokenForceTimeout'] 225 | _logger.warning( 226 | 'You have %d seconds to confirm control by pressing circle button on robot.', 227 | timeout) 228 | with connect(f'wss://{self._hostname}/desk/api/navigation/events', 229 | server_hostname='robot.franka.de', 230 | additional_headers={ 231 | 'authorization': 232 | self._session.cookies.get('authorization') 233 | }) as websocket: 234 | while True: 235 | event: typing.Dict = json_module.loads(websocket.recv(timeout)) 236 | if 'circle' in event.keys(): 237 | if event['circle']: 238 | break 239 | self._save_token( 240 | Token(str(response['id']), self._username, response['token'])) 241 | _logger.info('Taken control.') 242 | return True 243 | 244 | def release_control(self) -> None: 245 | """ 246 | Explicitly relinquish control of the Desk. This will allow 247 | other users to take control or transfer control to the next 248 | user if there is an active queue of control requests. 249 | """ 250 | if self._legacy: 251 | return 252 | _logger.info('Releasing control.') 253 | try: 254 | self._request('delete', 255 | '/admin/api/control-token', 256 | json={'token': self._token.token}) 257 | except ConnectionError as err: 258 | if 'ControlTokenUnknown' in str(err): 259 | _logger.warning('Control release failed. Not in control.') 260 | else: 261 | raise err 262 | self._token = Token() 263 | 264 | @staticmethod 265 | def encode_password(username: str, password: str) -> bytes: 266 | """ 267 | Encodes the password into the form needed to log into the Desk interface. 268 | """ 269 | bytes_str = ','.join([ 270 | str(b) for b in hashlib.sha256(( 271 | f'{password}#{username}@franka').encode('utf-8')).digest() 272 | ]) 273 | return base64.encodebytes(bytes_str.encode('utf-8')).decode('utf-8') 274 | 275 | def login(self) -> None: 276 | """ 277 | Uses the object's instance parameters to log into the Desk. 278 | The :py:class`Desk` class's constructor will try to connect 279 | and login automatically. 280 | """ 281 | login = self._request( 282 | 'post', 283 | '/admin/api/login', 284 | json={ 285 | 'login': self._username, 286 | 'password': self.encode_password(self._username, self._password) 287 | }) 288 | self._session.cookies.set('authorization', login.text) 289 | self._logged_in = True 290 | _logger.info('Login succesful.') 291 | 292 | def logout(self) -> None: 293 | """ 294 | Logs the current user out of the Desk. API calls will no longer 295 | be possible. 296 | """ 297 | self._request('post', '/admin/api/logout') 298 | self._session.cookies.clear() 299 | self._logged_in = False 300 | _logger.info('Logout successful.') 301 | 302 | def _get_active_token(self) -> Token: 303 | token = Token() 304 | if self._legacy: 305 | return Token() 306 | response = self._request('get', '/admin/api/control-token').json() 307 | if response['activeToken'] is not None: 308 | token.id = str(response['activeToken']['id']) 309 | token.owned_by = response['activeToken']['ownedBy'] 310 | return token 311 | 312 | def has_control(self) -> bool: 313 | """ 314 | Returns: 315 | bool: True if this instance is in control of the Desk. 316 | """ 317 | if self._legacy: 318 | return True 319 | return self._token.id == self._get_active_token().id 320 | 321 | def _request(self, 322 | method: typing.Literal['post', 'get', 'delete'], 323 | url: str, 324 | json: typing.Dict[str, str] = None, 325 | headers: typing.Dict[str, str] = None, 326 | files: typing.Dict[str, str] = None) -> requests.Response: 327 | fun = getattr(self._session, method) 328 | response: requests.Response = fun(parse.urljoin(f'https://{self._hostname}', 329 | url), 330 | json=json, 331 | headers=headers, 332 | files=files) 333 | if response.status_code != 200: 334 | raise ConnectionError(response.text) 335 | return response 336 | 337 | def _listen(self, cb, timeout): 338 | ctx = ssl.SSLContext(ssl.PROTOCOL_TLS_CLIENT) 339 | ctx.check_hostname = False 340 | ctx.verify_mode = ssl.CERT_NONE 341 | with connect( 342 | f'wss://{self._hostname}/desk/api/navigation/events', 343 | # server_hostname='robot.franka.de', 344 | ssl_context=ctx, 345 | additional_headers={ 346 | 'authorization': self._session.cookies.get('authorization') 347 | }) as websocket: 348 | self._listening = True 349 | while self._listening: 350 | try: 351 | event: typing.Dict = json_module.loads(websocket.recv(timeout)) 352 | cb(event) 353 | except TimeoutError: 354 | pass 355 | 356 | def listen(self, cb: typing.Callable[[typing.Dict], None]) -> None: 357 | """ 358 | Starts a thread listening to Pilot button events. All the Pilot buttons, 359 | except for the `Pilot Mode` button can be captured. Make sure Pilot Mode is 360 | set to Desk instead of End-Effector to receive direction key events. You can 361 | change the Pilot mode by pressing the `Pilot Mode` button or changing the mode 362 | in the Desk. Events will be triggered while buttons are pressed down or released. 363 | 364 | Args: 365 | cb: Callback fucntion that is called whenever a button event is received from the 366 | Desk. The callback receives a dict argument that contains the triggered buttons 367 | as keys. The values of those keys will depend on the kind of event, either True 368 | for a button pressed down or False when released. 369 | The possible buttons are: `circle`, `cross`, `check`, `left`, `right`, `down`, 370 | and `up`. 371 | """ 372 | self._listen_thread = threading.Thread(target=self._listen, args=(cb, 1.0)) 373 | self._listen_thread.start() 374 | 375 | def stop_listen(self) -> None: 376 | """ 377 | Stop listener thread (cf. :py:func:`panda_py.Desk.listen`). 378 | """ 379 | self._listening = False 380 | if self._listen_thread is not None: 381 | self._listen_thread.join() 382 | -------------------------------------------------------------------------------- /src/panda_py/__init__.pyi: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Introduction 4 | ------------ 5 | 6 | panda-py is a Python library for the Franka Emika Robot System 7 | that allows you to program and control the robot in real-time. 8 | 9 | 10 | """ 11 | from __future__ import annotations 12 | 13 | import base64 as base64 14 | import configparser as configparser 15 | import dataclasses as dataclasses 16 | import hashlib as hashlib 17 | import json as json_module 18 | import logging as logging 19 | import os as os 20 | import ssl as ssl 21 | import threading as threading 22 | import typing as typing 23 | from urllib import parse 24 | 25 | import requests as requests 26 | import urllib3 as urllib3 27 | from websockets.sync.client import connect 28 | 29 | from panda_py._core import Panda, PandaContext, fk, ik, ik_full 30 | 31 | from . import _core, libfranka 32 | 33 | __all__: list = [ 34 | 'Panda', 'PandaContext', 'constants', 'controllers', 'libfranka', 'motion', 35 | 'fk', 'ik', 'ik_full', 'Desk', 'TOKEN_PATH' 36 | ] 37 | 38 | 39 | class Desk: 40 | """ 41 | 42 | Connects to the control unit running the web-based Desk interface 43 | to manage the robot. Use this class to interact with the Desk 44 | from Python, e.g. if you use a headless setup. This interface 45 | supports common tasks such as unlocking the brakes, activating 46 | the FCI etc. 47 | 48 | Newer versions of the system software use role-based access 49 | management to allow only one user to be in control of the Desk 50 | at a time. The controlling user is authenticated using a token. 51 | The :py:class:`Desk` class saves those token in :py:obj:`TOKEN_PATH` 52 | and will use them when reconnecting to the Desk, retaking control. 53 | Without a token, control of a Desk can only be taken, if there is 54 | no active claim or the controlling user explicitly relinquishes control. 55 | If the controlling user's token is lost, a user can take control 56 | forcefully (cf. :py:func:`Desk.take_control`) but needs to confirm 57 | physical access to the robot by pressing the circle button on the 58 | robot's Pilot interface. 59 | 60 | """ 61 | 62 | @staticmethod 63 | def encode_password(username: str, password: str) -> bytes: 64 | """ 65 | 66 | Encodes the password into the form needed to log into the Desk interface. 67 | 68 | """ 69 | 70 | def __init__(self, 71 | hostname: str, 72 | username: str, 73 | password: str, 74 | platform: str = 'panda') -> None: 75 | ... 76 | 77 | def _get_active_token(self) -> Token: 78 | ... 79 | 80 | def _listen(self, cb, timeout): 81 | ... 82 | 83 | def _load_token(self) -> Token: 84 | ... 85 | 86 | def _request(self, 87 | method: typing.Literal['post', 'get', 'delete'], 88 | url: str, 89 | json: typing.Dict[str, str] = None, 90 | headers: typing.Dict[str, str] = None, 91 | files: typing.Dict[str, str] = None) -> requests.models.Response: 92 | ... 93 | 94 | def _save_token(self, token: Token) -> None: 95 | ... 96 | 97 | def activate_fci(self) -> None: 98 | """ 99 | 100 | Activates the Franka Research Interface (FCI). Note that the 101 | brakes must be unlocked first. For older Desk versions, this 102 | function does nothing. 103 | 104 | """ 105 | 106 | def deactivate_fci(self) -> None: 107 | """ 108 | 109 | Deactivates the Franka Research Interface (FCI). For older 110 | Desk versions, this function does nothing. 111 | 112 | """ 113 | 114 | def has_control(self) -> bool: 115 | """ 116 | 117 | Returns: 118 | bool: True if this instance is in control of the Desk. 119 | 120 | """ 121 | 122 | def listen(self, cb: typing.Callable[[typing.Dict], NoneType]) -> None: 123 | """ 124 | 125 | Starts a thread listening to Pilot button events. All the Pilot buttons, 126 | except for the `Pilot Mode` button can be captured. Make sure Pilot Mode is 127 | set to Desk instead of End-Effector to receive direction key events. You can 128 | change the Pilot mode by pressing the `Pilot Mode` button or changing the mode 129 | in the Desk. Events will be triggered while buttons are pressed down or released. 130 | 131 | Args: 132 | cb: Callback fucntion that is called whenever a button event is received from the 133 | Desk. The callback receives a dict argument that contains the triggered buttons 134 | as keys. The values of those keys will depend on the kind of event, either True 135 | for a button pressed down or False when released. 136 | The possible buttons are: `circle`, `cross`, `check`, `left`, `right`, `down`, 137 | and `up`. 138 | 139 | """ 140 | 141 | def lock(self, force: bool = True) -> None: 142 | """ 143 | 144 | Locks the brakes. API call blocks until the brakes are locked. 145 | 146 | """ 147 | 148 | def login(self) -> None: 149 | """ 150 | 151 | Uses the object's instance parameters to log into the Desk. 152 | The :py:class`Desk` class's constructor will try to connect 153 | and login automatically. 154 | 155 | """ 156 | 157 | def logout(self) -> None: 158 | """ 159 | 160 | Logs the current user out of the Desk. API calls will no longer 161 | be possible. 162 | 163 | """ 164 | 165 | def reboot(self) -> None: 166 | """ 167 | 168 | Reboots the robot hardware (this will close open connections). 169 | 170 | """ 171 | 172 | def release_control(self) -> None: 173 | """ 174 | 175 | Explicitly relinquish control of the Desk. This will allow 176 | other users to take control or transfer control to the next 177 | user if there is an active queue of control requests. 178 | 179 | """ 180 | 181 | def stop_listen(self) -> None: 182 | """ 183 | 184 | Stop listener thread (cf. :py:func:`panda_py.Desk.listen`). 185 | 186 | """ 187 | 188 | def take_control(self, force: bool = False) -> bool: 189 | """ 190 | 191 | Takes control of the Desk, generating a new control token and saving it. 192 | If `force` is set to True, control can be taken forcefully even if another 193 | user is already in control. However, the user will have to press the circle 194 | button on the robot's Pilot within an alotted amount of time to confirm 195 | physical access. 196 | 197 | For legacy versions of the Desk, this function does nothing. 198 | 199 | """ 200 | 201 | def unlock(self, force: bool = True) -> None: 202 | """ 203 | 204 | Unlocks the brakes. API call blocks until the brakes are unlocked. 205 | 206 | """ 207 | 208 | 209 | class Token: 210 | """ 211 | 212 | Represents a Desk token owned by a user. 213 | 214 | """ 215 | __dataclass_fields__: typing.ClassVar[ 216 | dict] # value = {'id': Field(name='id',type=,default='',default_factory=,init=True,repr=True,hash=None,compare=True,metadata=mappingproxy({}),kw_only=False,_field_type=_FIELD), 'owned_by': Field(name='owned_by',type=,default='',default_factory=,init=True,repr=True,hash=None,compare=True,metadata=mappingproxy({}),kw_only=False,_field_type=_FIELD), 'token': Field(name='token',type=,default='',default_factory=,init=True,repr=True,hash=None,compare=True,metadata=mappingproxy({}),kw_only=False,_field_type=_FIELD)} 217 | __dataclass_params__: typing.ClassVar[ 218 | dataclasses. 219 | _DataclassParams] # value = _DataclassParams(init=True,repr=True,eq=True,order=False,unsafe_hash=False,frozen=False,match_args=True,kw_only=False,slots=False,weakref_slot=False) 220 | __hash__: typing.ClassVar[None] = None 221 | __match_args__: typing.ClassVar[tuple] = ('id', 'owned_by', 'token') 222 | id: typing.ClassVar[str] = '' 223 | owned_by: typing.ClassVar[str] = '' 224 | token: typing.ClassVar[str] = '' 225 | 226 | def __eq__(self, other): 227 | ... 228 | 229 | def __init__(self, id: str = '', owned_by: str = '', token: str = '') -> None: 230 | ... 231 | 232 | def __repr__(self): 233 | ... 234 | 235 | 236 | TOKEN_PATH: str = '~/.panda_py/token.conf' 237 | __version__: str = '0.8.1' 238 | _logger: logging.Logger # value = 239 | -------------------------------------------------------------------------------- /src/panda_py/cli.py: -------------------------------------------------------------------------------- 1 | """ 2 | This submodule contains convenience functions that are installed 3 | as executables. These executables can be integrated into bash 4 | scripts, alias commands etc. The table below lists the available 5 | executables and the corresponding entry-points. 6 | 7 | +-----------------------+----------------------------+ 8 | | Executable | Entry-point | 9 | +=======================+============================+ 10 | | panda-unlock | :py:func:`unlock` | 11 | +-----------------------+----------------------------+ 12 | | panda-lock | :py:func:`lock` | 13 | +-----------------------+----------------------------+ 14 | | panda-reboot | :py:func:`reboot` | 15 | +-----------------------+----------------------------+ 16 | | panda-take-control | :py:func:`take_control` | 17 | +-----------------------+----------------------------+ 18 | | panda-release-control | :py:func:`release_control` | 19 | +-----------------------+----------------------------+ 20 | 21 | """ 22 | import argparse 23 | 24 | from . import Desk 25 | 26 | 27 | def _create_argument_parser(needs_platform: bool = True) -> argparse.ArgumentParser: 28 | parser = argparse.ArgumentParser() 29 | parser.add_argument('host', type=str, help='Robot Desk IP or hostname.') 30 | parser.add_argument('user', type=str, help='Desk username.') 31 | parser.add_argument('password', type=str, help='Desk password.') 32 | if needs_platform: 33 | parser.add_argument('--platform', type=str, default='panda', help='Platform of robot, i.e. panda (default) or fr3.') 34 | return parser 35 | 36 | 37 | def unlock(): 38 | """ 39 | Unlocks the robot's brakes and activates the FCI. 40 | 41 | Args: 42 | host: IP or hostname of the control unit running the Desk. 43 | user: Username used to log into the Desk. 44 | password: Password of the given username. 45 | platform: The targeted robot platform, i.e. panda or fr3. 46 | """ 47 | parser = _create_argument_parser() 48 | args = parser.parse_args() 49 | 50 | desk = Desk(args.host, args.user, args.password, platform=args.platform) 51 | desk.unlock() 52 | desk.activate_fci() 53 | 54 | 55 | def lock(): 56 | """ 57 | Locks the robot's brakes and deactivates the FCI. 58 | 59 | Args: 60 | host: IP or hostname of the control unit running the Desk. 61 | user: Username used to log into the Desk. 62 | password: Password of the given username. 63 | platform: The targeted robot platform, i.e. panda or fr3. 64 | """ 65 | parser = _create_argument_parser() 66 | args = parser.parse_args() 67 | 68 | desk = Desk(args.host, args.user, args.password, platform=args.platform) 69 | desk.lock() 70 | desk.deactivate_fci() 71 | 72 | 73 | def reboot(): 74 | """ 75 | Reboots the robot. Current versions of the robot's Desk software 76 | will eventually hang up when running the FCI continuously for 77 | several days. This function can be conveniently integrated 78 | into cronjobs or similar to regularly reboot the robot. 79 | 80 | Args: 81 | host: IP or hostname of the control unit running the Desk. 82 | user: Username used to log into the Desk. 83 | password: Password of the given username. 84 | """ 85 | parser = _create_argument_parser(needs_platform=False) 86 | args = parser.parse_args() 87 | 88 | desk = Desk(args.host, args.user, args.password) 89 | desk.reboot() 90 | 91 | 92 | def take_control(): 93 | """ 94 | Take control of the Desk with the given user credentials. 95 | 96 | Args: 97 | host: IP or hostname of the control unit running the Desk. 98 | user: Username used to log into the Desk. 99 | password: Password of the given username. 100 | force: Forcibly take control from another active user 101 | by pressing circle button on physical robot. 102 | """ 103 | parser = _create_argument_parser(needs_platform=False) 104 | parser.add_argument('--force', action='store_true', help='Force takeover if another user is in control.') 105 | args = parser.parse_args() 106 | 107 | desk = Desk(args.host, args.user, args.password) 108 | desk.take_control(force=args.force) 109 | 110 | 111 | def release_control(): 112 | """ 113 | Release control of the desk. This will allow another user to 114 | take control without needing physical access to the robot. 115 | 116 | Args: 117 | host: IP or hostname of the control unit running the Desk. 118 | user: Username used to log into the Desk. 119 | password: Password of the given username. 120 | force: Forcibly take control from another active user 121 | by pressing circle button on physical robot. 122 | """ 123 | parser = _create_argument_parser(needs_platform=False) 124 | args = parser.parse_args() 125 | 126 | desk = Desk(args.host, args.user, args.password) 127 | desk.release_control() 128 | -------------------------------------------------------------------------------- /src/panda_py/constants.py: -------------------------------------------------------------------------------- 1 | """ 2 | Commonly used constants. 3 | """ 4 | 5 | # pylint: disable=no-name-in-module 6 | from ._core import _JOINT_POSITION_START, _JOINT_LIMITS_LOWER, _JOINT_LIMITS_UPPER 7 | 8 | __all__ = ['JOINT_POSITION_START', 'JOINT_LIMITS_LOWER', 'JOINT_LIMITS_UPPER'] 9 | 10 | JOINT_POSITION_START = _JOINT_POSITION_START 11 | """ 12 | Common start pose of the robot. State-space around this pose has 13 | high manipulability, reachability, distance to joint limits etc. 14 | """ 15 | 16 | JOINT_LIMITS_LOWER = _JOINT_LIMITS_LOWER 17 | """ 18 | Lower joint position limits in radian. 19 | """ 20 | 21 | JOINT_LIMITS_UPPER = _JOINT_LIMITS_UPPER 22 | """ 23 | Upper joint position limits in radian. 24 | """ 25 | -------------------------------------------------------------------------------- /src/panda_py/controllers.py: -------------------------------------------------------------------------------- 1 | """ 2 | Control library for the Panda robot. The general workflow 3 | is to instantiate a controller and hand it over to the 4 | :py:class:`panda_py.Panda` class for execution using the 5 | function :py:func:`panda_py.Panda.start_controller`. 6 | """ 7 | 8 | # pylint: disable=no-name-in-module 9 | from ._core import AppliedForce, AppliedTorque,\ 10 | CartesianImpedance, Force, IntegratedVelocity,\ 11 | JointPosition, TorqueController 12 | 13 | __all__ = [ 14 | 'TorqueController', 'CartesianImpedance', 'IntegratedVelocity', 15 | 'JointPosition', 'AppliedTorque', 'AppliedForce', 'Force' 16 | ] 17 | -------------------------------------------------------------------------------- /src/panda_py/motion.py: -------------------------------------------------------------------------------- 1 | """ 2 | Motion generation for the Panda robot. These are also directly 3 | integrated as convenience methods of the :py:class:`panda_py.Panda` class. 4 | """ 5 | 6 | # pylint: disable=no-name-in-module 7 | from ._core import JointTrajectory, CartesianTrajectory 8 | 9 | __all__ = ['JointTrajectory', 'CartesianTrajectory'] 10 | -------------------------------------------------------------------------------- /src/panda_py/py.typed: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JeanElsner/panda-py/c480378d84e403091b639b46b1aa95eac61da282/src/panda_py/py.typed --------------------------------------------------------------------------------