├── .ci ├── common.sh ├── conda.sh ├── static.sh └── test.sh ├── .codecov.yml ├── .gitignore ├── .gitlint ├── .nengobones.yml ├── .pre-commit-config.yaml ├── .templates └── LICENSE.rst.template ├── .travis.yml ├── CHANGES.rst ├── CONTRIBUTING.rst ├── LICENSE.rst ├── MANIFEST.in ├── README.rst ├── abr_control ├── __init__.py ├── _vendor │ ├── README.rst │ ├── __init__.py │ ├── nengolib │ │ ├── __init__.py │ │ └── stats │ │ │ ├── __init__.py │ │ │ ├── ntmdists.py │ │ │ └── ortho.py │ └── requirements.txt ├── arms │ ├── __init__.py │ ├── base_config.py │ ├── jaco2 │ │ ├── __init__.py │ │ ├── config.py │ │ └── jaco2.xml │ ├── mujoco_config.py │ ├── onejoint │ │ ├── __init__.py │ │ ├── balljoint.xml │ │ ├── config.py │ │ └── onejoint.xml │ ├── tests │ │ ├── __init__.py │ │ ├── dummy_base_arm.py │ │ ├── dummy_mujoco_arm.py │ │ ├── test_base_config.py │ │ └── test_mujoco_config.py │ ├── threejoint │ │ ├── __init__.py │ │ ├── arm_files │ │ │ ├── __init__.py │ │ │ ├── arm3link.msim │ │ │ ├── mplshlib.h │ │ │ ├── mpltable.h │ │ │ ├── py3LinkArm.cpp │ │ │ ├── py3LinkArm.pyx │ │ │ ├── py3LinkArm.pyxbld │ │ │ └── threelinkarm.cpp │ │ ├── arm_sim.py │ │ ├── config.py │ │ └── threejoint.xml │ ├── twojoint │ │ ├── __init__.py │ │ ├── arm_sim.py │ │ ├── config.py │ │ └── twojoint.xml │ └── ur5 │ │ ├── __init__.py │ │ ├── config.py │ │ └── ur5.xml ├── controllers │ ├── __init__.py │ ├── avoid_joint_limits.py │ ├── avoid_obstacles.py │ ├── controller.py │ ├── damping.py │ ├── floating.py │ ├── joint.py │ ├── osc.py │ ├── path_planners │ │ ├── __init__.py │ │ ├── inverse_kinematics.py │ │ ├── orientation.py │ │ ├── path_planner.py │ │ ├── position_profiles.py │ │ └── velocity_profiles.py │ ├── resting_config.py │ ├── signals │ │ ├── __init__.py │ │ ├── dynamics_adaptation.py │ │ └── tests │ │ │ └── test_dynamics_adaptation.py │ ├── sliding.py │ └── tests │ │ └── test_osc.py ├── interfaces │ ├── __init__.py │ ├── coppeliasim.py │ ├── coppeliasim_files │ │ ├── __init__.py │ │ ├── remoteApi.dll │ │ ├── remoteApi.dylib │ │ ├── remoteApi.so │ │ ├── sim.py │ │ └── simConst.py │ ├── interface.py │ ├── mujoco.py │ └── pygame.py ├── utils │ ├── __init__.py │ ├── colors.py │ ├── download_meshes.py │ ├── os_utils.py │ ├── paths.py │ └── transformations.py └── version.py ├── examples ├── CoppeliaSim │ ├── force_floating_control.py │ ├── force_osc_abg.py │ ├── force_osc_xyz.py │ ├── force_osc_xyz_avoid_obstacle.py │ ├── force_osc_xyz_dynamics_adaptation.py │ ├── force_osc_xyzabg.py │ ├── force_osc_xyzabg_linear_position_gaussian_velocity.py │ ├── force_sliding_control_xyz.py │ └── position_joint_control.py ├── Mujoco │ ├── force_floating_control.py │ ├── force_joint_control.py │ ├── force_joint_control_balljoint.py │ ├── force_joint_control_two_balljoints.py │ ├── force_osc_abg.py │ ├── force_osc_xyz.py │ ├── force_osc_xyz_balljoint.py │ ├── force_osc_xyz_dynamics_adaptation.py │ ├── force_osc_xyz_geometric_arm.py │ ├── force_osc_xyz_geometric_arm_linear_position_gaussian_velocity.py │ ├── force_osc_xyz_linear_path_gaussian_velocity.py │ ├── force_osc_xyzabg.py │ ├── force_osc_xyzabg_linear_position_gaussian_velocity.py │ ├── mujoco_balljoint.xml │ ├── mujoco_two_balljoints.xml │ ├── position_joint_control_inverse_kinematics.py │ ├── rover.xml │ └── rover_vision.py ├── PyGame │ ├── avoid_obstacles.py │ ├── force_osc_g.py │ ├── force_osc_xy.py │ ├── force_osc_xy_avoid_joint_limits.py │ ├── force_osc_xy_dynamics_adaptation.py │ ├── force_osc_xy_ellipse_position_linear_velocity.py │ ├── force_osc_xy_integrated_error.py │ ├── force_osc_xy_linear_position_gaussian_velocity.py │ ├── force_osc_xy_linear_position_linear_velocity.py │ ├── force_osc_xyg.py │ ├── force_sliding_xy.py │ ├── force_sliding_xy_dynamics_adaptation.py │ └── position_joint_control_inverse_kinematics.py ├── ellipse_position_profile.png ├── linear_path_gauss_velocity.png ├── linear_path_linear_velocity.png ├── linear_position_profile.png ├── path_planning │ ├── ellipse_position_linear_velocity.py │ ├── from_points_position_gauss_velocity.py │ ├── linear_position_gauss_velocity.py │ ├── linear_position_gauss_velocity_successive_target.py │ ├── linear_position_linear_velocity.py │ ├── non-zero_target_velocity.py │ └── sin_position_linear_velocity.py ├── timing.png └── timing_plots.py ├── pyproject.toml ├── setup.cfg └── setup.py /.ci/common.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # Small snippets common to all CI scripts. 3 | # All CI scripts should source this script. 4 | 5 | STATUS=0 # used to exit with non-zero status if any command fails 6 | 7 | exe() { 8 | echo "\$ $*"; 9 | # shellcheck disable=SC2034 10 | "$@" || STATUS=1; 11 | } 12 | -------------------------------------------------------------------------------- /.ci/conda.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | if [[ ! -e .ci/common.sh || ! -e abr_control ]]; then 3 | echo "Run this script from the root directory of this repository" 4 | exit 1 5 | fi 6 | source .ci/common.sh 7 | 8 | # This script sets up the conda environment for all the other scripts 9 | 10 | NAME=$0 11 | COMMAND=$1 12 | MINICONDA="http://repo.continuum.io/miniconda/Miniconda-latest-Linux-x86_64.sh" 13 | 14 | if [[ "$COMMAND" == "install" ]]; then 15 | export PATH="$HOME/miniconda/bin:$PATH" 16 | if ! [[ -d $HOME/miniconda/envs/test ]]; then 17 | exe rm -rf "$HOME/miniconda" 18 | exe wget "$MINICONDA" --quiet -O miniconda.sh 19 | exe bash miniconda.sh -b -p "$HOME/miniconda" 20 | exe conda create --quiet -y -n test python="$PYTHON" pip 21 | fi 22 | exe conda config --set always_yes yes --set changeps1 no 23 | exe conda update -q conda 24 | exe conda info -a 25 | exe source activate test 26 | exe pip install pip 27 | exe pip install "git+https://github.com/nengo/pytest-plt.git" 28 | elif [[ "$COMMAND" == "before_cache" ]]; then 29 | conda clean --all 30 | elif [[ -z "$COMMAND" ]]; then 31 | echo "$NAME requires a command like 'install' or 'script'" 32 | else 33 | echo "$NAME does not define $COMMAND" 34 | fi 35 | -------------------------------------------------------------------------------- /.ci/static.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # Automatically generated by nengo-bones, do not edit this file directly 4 | 5 | NAME=$0 6 | COMMAND=$1 7 | STATUS=0 # used to exit with non-zero status if any command fails 8 | 9 | exe() { 10 | echo "\$ $*"; 11 | # remove empty spaces from args 12 | args=( "$@" ) 13 | for i in "${!args[@]}"; do 14 | [ -n "${args[$i]}" ] || unset "args[$i]" 15 | done 16 | "${args[@]}" || { echo -e "\033[1;31mCOMMAND '${args[0]}' FAILED\033[0m"; STATUS=1; } 17 | } 18 | 19 | if [[ ! -e abr_control ]]; then 20 | echo "Run this script from the root directory of this repository" 21 | exit 1 22 | fi 23 | 24 | 25 | shopt -s globstar 26 | 27 | if [[ "$COMMAND" == "install" ]]; then 28 | : 29 | 30 | 31 | exe pip install "jupyter>=1.0.0" "pylint>=1.9.2" "codespell>=1.12.0" "gitlint>=0.1.2" "collective.checkdocs>=0.2" "flake8>=3.7.7" 32 | elif [[ "$COMMAND" == "before_script" ]]; then 33 | : 34 | elif [[ "$COMMAND" == "script" ]]; then 35 | 36 | exe pylint abr_control --rcfile=setup.cfg --jobs=0 37 | exe flake8 abr_control 38 | if ls docs/**/*.ipynb &>/dev/null; then 39 | exe jupyter nbconvert \ 40 | --log-level WARN \ 41 | --to python \ 42 | --TemplateExporter.exclude_input_prompt=True \ 43 | --TemplateExporter.exclude_markdown=True \ 44 | -- docs/**/*.ipynb 45 | # Remove style issues introduced in the conversion: 46 | # s/# $/#/g replaces lines with just '# ' with '#' 47 | # /get_ipython()/d removes lines containing 'get_ipython()' 48 | # $ d removes a trailing newline 49 | for nb_file in docs/**/*.ipynb; do 50 | sed -i \ 51 | -e 's/# $/#/g' \ 52 | -e '/get_ipython()/d' \ 53 | -e '$ d' \ 54 | -- "${nb_file%.ipynb}.py" 55 | done 56 | fi 57 | if [[ "$(python -c 'import sys; print(sys.version_info >= (3, 6))')" == "True" ]]; then 58 | exe black --check abr_control 59 | fi 60 | exe pylint docs --rcfile=setup.cfg --jobs=0 --disable=missing-docstring,trailing-whitespace,wrong-import-position,unnecessary-semicolon 61 | exe flake8 docs --extend-ignore=E402,E703,W291,W293,W391 62 | for nb_file in docs/**/*.ipynb; do 63 | rm "${nb_file%.ipynb}.py" 64 | done 65 | exe codespell -q 3 \ 66 | --skip="./build,*/_build,*-checkpoint.ipynb,./.eggs,./.git,*/_vendor" \ 67 | --ignore-words-list="DOF,dof,hist,nd,compiletime" 68 | exe shellcheck -e SC2087 .ci/*.sh 69 | # undo single-branch cloning 70 | git config --replace-all remote.origin.fetch +refs/heads/*:refs/remotes/origin/* 71 | git fetch origin master 72 | N_COMMITS=$(git rev-list --count HEAD ^origin/master) 73 | for ((i=0; i=0.8.0 22 | - cython>=0.29.0 23 | - matplotlib>=3.0.0 24 | - numpy>=1.16.0 25 | - setuptools>=18.0 26 | - scipy>=1.1.0 27 | - sympy>=1.3 28 | tests_req: 29 | - pytest>=4.4.0 30 | - pytest-xdist>=1.26.0 31 | - pytest-cov>=2.6.0 32 | - pytest-plt 33 | - coverage>=4.5.0 34 | - nengo>=2.8.0 35 | classifiers: 36 | - "Development Status :: 5 - Production/Stable" 37 | - "Framework :: ABR Control" 38 | - "Intended Audience :: Science/Research" 39 | - "Operating System :: OS Independent" 40 | - "Programming Language :: Python :: 3 :: Only" 41 | - "Programming Language :: Python :: 3.6" 42 | - "Programming Language :: Python :: 3.7" 43 | - "Topic :: Scientific/Engineering :: Artificial Intelligence" 44 | 45 | setup_cfg: 46 | codespell: 47 | ignore_words: 48 | - DOF 49 | - dof 50 | - hist 51 | - nd 52 | - compiletime 53 | coverage: 54 | omit_files: 55 | - "abr_control/_vendor/*" 56 | - "abr_control/interfaces/coppeliasim_files/*" 57 | - "abr_control/arms/threejoint/arm_files/*" 58 | flake8: 59 | exclude: 60 | - "abr_control/_vendor/*" 61 | - "abr_control/interfaces/coppeliasim_files/*" 62 | - "abr_control/arms/threejoint/arm_files/*" 63 | - "abr_control/utils/transformations.py" 64 | ignore: 65 | - C901 66 | - E402 67 | - E722 68 | pytest: 69 | norecursedirs: 70 | - examples 71 | - _vendor 72 | pylint: 73 | ignore: 74 | - _vendor 75 | - arm_files 76 | - coppeliasim_files 77 | - transformations.py 78 | disable: 79 | - F0001 80 | - C0116 81 | - C0115 82 | - C0114 83 | - too-many-blank-lines 84 | - W504 85 | - W0621 86 | - W0702 87 | known_third_party: 88 | - mpl_toolkits 89 | - nengo 90 | - nengolib 91 | - scipy 92 | 93 | travis_yml: 94 | jobs: 95 | - script: static 96 | - script: test-coverage 97 | test_args: --plots 98 | pypi_user: tbekolay 99 | deploy_dists: 100 | - sdist 101 | - bdist_wheel 102 | 103 | ci_scripts: 104 | - template: test 105 | output_name: test-coverage 106 | coverage: true 107 | 108 | codecov_yml: {} 109 | 110 | pre_commit_config_yaml: {} 111 | pyproject_toml: {} 112 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | # Automatically generated by nengo-bones, do not edit this file directly 2 | 3 | repos: 4 | - repo: https://github.com/psf/black 5 | rev: 21.12b0 6 | hooks: 7 | - id: black 8 | files: \.py$ 9 | - repo: https://github.com/pycqa/isort 10 | rev: 5.6.4 11 | hooks: 12 | - id: isort 13 | files: \.py$ 14 | -------------------------------------------------------------------------------- /.templates/LICENSE.rst.template: -------------------------------------------------------------------------------- 1 | {% include "templates/LICENSE.rst.template" %} 2 | 3 | Licensed code 4 | ============= 5 | 6 | ABR Control imports several open source libraries: 7 | 8 | * `NumPy `_ - Used under 9 | `BSD license `__ 10 | * `Cython `_ - Used under 11 | `Apache license `__ 12 | * `setuptools `_ - Used under 13 | `MIT license `__ 14 | * `SciPy `_ - Used under 15 | `BSD 3-Clause license `__ 16 | * `Cloudpickle `_ - Used under 17 | `Custom license `__ 18 | * `SymPy `_ - Used under 19 | `Custom license `__ 20 | * `matplotlib `_ - Used under 21 | `modified PSF license `__ 22 | 23 | ABR_Control also includes code modified from other libraries. 24 | 25 | The file ``abr_control/utils/transformations.py`` contains code with the 26 | following license: 27 | 28 | Copyright (c) 2006-2015, Christoph Gohlke 29 | Copyright (c) 2006-2015, The Regents of the University of California 30 | Produced at the Laboratory for Fluorescence Dynamics 31 | All rights reserved. 32 | 33 | 34 | To run the tests, ABR Control uses: 35 | 36 | * `codespell `_ - Used under 37 | `GPL license `__ 38 | * `pylint `_ - Used under 39 | `GPL license `__ 40 | * `pytest `_ - Used under 41 | `MIT license `__ 42 | * `pytest-cov `_ - Used under 43 | `MIT license `__ 44 | * `pytest-rng `__ Used under 45 | `MIT license `__ 46 | * `pytest-xdist `_ - Used under 47 | `MIT license `__ 48 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # Automatically generated by nengo-bones, do not edit this file directly 2 | 3 | language: python 4 | python: 3.6 5 | notifications: 6 | email: 7 | on_success: change 8 | on_failure: change 9 | cache: pip 10 | 11 | dist: xenial 12 | 13 | env: 14 | global: 15 | - SCRIPT="test" 16 | - TEST_ARGS="" 17 | - BRANCH_NAME="${TRAVIS_PULL_REQUEST_BRANCH:-$TRAVIS_BRANCH}" 18 | 19 | jobs: 20 | include: 21 | - 22 | env: 23 | SCRIPT="static" 24 | - 25 | env: 26 | SCRIPT="test-coverage" 27 | TEST_ARGS="--plots" 28 | - stage: deploy 29 | if: branch =~ ^release-candidate-* OR tag =~ ^v[0-9]* 30 | env: SCRIPT="deploy" 31 | cache: false 32 | deploy: 33 | - provider: pypi 34 | server: https://test.pypi.org/legacy/ 35 | user: tbekolay 36 | password: $PYPI_TEST_TOKEN 37 | distributions: "sdist bdist_wheel " 38 | on: 39 | all_branches: true 40 | tags: false 41 | condition: $TRAVIS_BRANCH =~ ^release-candidate-* 42 | - provider: pypi 43 | user: tbekolay 44 | password: $PYPI_TOKEN 45 | distributions: "sdist bdist_wheel " 46 | on: 47 | all_branches: true 48 | tags: true 49 | condition: $TRAVIS_TAG =~ ^v[0-9]* 50 | 51 | before_install: 52 | # export travis_terminate for use in scripts, from here: 53 | # https://github.com/travis-ci/travis-build/blob/master/lib/travis/build/bash/travis_terminate.bash 54 | - export -f travis_terminate 55 | _travis_terminate_agent 56 | _travis_terminate_freebsd 57 | _travis_terminate_linux 58 | _travis_terminate_osx 59 | _travis_terminate_unix 60 | _travis_terminate_windows 61 | # upgrade pip 62 | - pip install pip --upgrade 63 | # install/run nengo-bones 64 | - pip install git+https://github.com/nengo/nengo-bones#egg=nengo-bones 65 | - bones-generate --output-dir .ci ci-scripts 66 | - bones-check --verbose 67 | # display environment info 68 | - pip freeze 69 | 70 | install: 71 | - .ci/$SCRIPT.sh install 72 | - pip freeze 73 | 74 | before_script: 75 | - .ci/$SCRIPT.sh before_script 76 | 77 | script: 78 | - .ci/$SCRIPT.sh script 79 | 80 | before_cache: 81 | - .ci/$SCRIPT.sh before_cache 82 | 83 | after_success: 84 | - .ci/$SCRIPT.sh after_success 85 | 86 | after_failure: 87 | - .ci/$SCRIPT.sh after_failure 88 | 89 | before_deploy: 90 | - .ci/$SCRIPT.sh before_deploy 91 | 92 | after_deploy: 93 | - .ci/$SCRIPT.sh after_deploy 94 | 95 | after_script: 96 | - .ci/$SCRIPT.sh after_script 97 | -------------------------------------------------------------------------------- /CHANGES.rst: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abr/abr_control/0f6e47641d9299422fc24e09ee1445877c235cdc/CHANGES.rst -------------------------------------------------------------------------------- /CONTRIBUTING.rst: -------------------------------------------------------------------------------- 1 | .. Automatically generated by nengo-bones, do not edit this file directly 2 | 3 | *************************** 4 | Contributing to ABR Control 5 | *************************** 6 | 7 | Issues and pull requests are always welcome! 8 | We appreciate help from the community to make ABR Control better. 9 | 10 | Filing issues 11 | ============= 12 | 13 | If you find a bug in ABR Control, 14 | or think that a certain feature is missing, 15 | please consider 16 | `filing an issue `_! 17 | Please search the currently open issues first 18 | to see if your bug or feature request already exists. 19 | If so, feel free to add a comment to the issue 20 | so that we know that multiple people are affected. 21 | 22 | Making pull requests 23 | ==================== 24 | 25 | If you want to fix a bug or add a feature to ABR Control, 26 | we welcome pull requests. 27 | Ensure that you fill out all sections of the pull request template, 28 | deleting the comments as you go. 29 | 30 | Contributor agreement 31 | ===================== 32 | 33 | We require that all contributions be covered under 34 | our contributor assignment agreement. Please see 35 | `the agreement `_ 36 | for instructions on how to sign. 37 | 38 | More details 39 | ============ 40 | 41 | For more details on how to contribute to Nengo, 42 | please see the `developer guide `_. 43 | -------------------------------------------------------------------------------- /LICENSE.rst: -------------------------------------------------------------------------------- 1 | .. Automatically generated by nengo-bones, do not edit this file directly 2 | 3 | ******************* 4 | ABR Control license 5 | ******************* 6 | 7 | Copyright (c) 2017-2022 Applied Brain Research 8 | 9 | ABR Control is made available under a proprietary license 10 | that permits using, copying, sharing, and making derivative works from 11 | ABR Control and its source code for any non-commercial purpose, 12 | as long as the above copyright notice and this permission notice 13 | are included in all copies or substantial portions of the software. 14 | 15 | If you would like to use ABR Control commercially, 16 | licenses can be purchased from Applied Brain Research. 17 | Please contact travis.dewolf@appliedbrainresearch.com for more information. 18 | 19 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 20 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 21 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 22 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 23 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 24 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 25 | SOFTWARE. 26 | 27 | Licensed code 28 | ============= 29 | 30 | ABR Control imports several open source libraries: 31 | 32 | * `NumPy `_ - Used under 33 | `BSD license `__ 34 | * `Cython `_ - Used under 35 | `Apache license `__ 36 | * `setuptools `_ - Used under 37 | `MIT license `__ 38 | * `SciPy `_ - Used under 39 | `BSD 3-Clause license `__ 40 | * `Cloudpickle `_ - Used under 41 | `Custom license `__ 42 | * `SymPy `_ - Used under 43 | `Custom license `__ 44 | * `matplotlib `_ - Used under 45 | `modified PSF license `__ 46 | 47 | ABR_Control also includes code modified from other libraries. 48 | 49 | The file ``abr_control/utils/transformations.py`` contains code with the 50 | following license: 51 | 52 | Copyright (c) 2006-2015, Christoph Gohlke 53 | Copyright (c) 2006-2015, The Regents of the University of California 54 | Produced at the Laboratory for Fluorescence Dynamics 55 | All rights reserved. 56 | 57 | 58 | To run the tests, ABR Control uses: 59 | 60 | * `codespell `_ - Used under 61 | `GPL license `__ 62 | * `pylint `_ - Used under 63 | `GPL license `__ 64 | * `pytest `_ - Used under 65 | `MIT license `__ 66 | * `pytest-cov `_ - Used under 67 | `MIT license `__ 68 | * `pytest-rng `__ Used under 69 | `MIT license `__ 70 | * `pytest-xdist `_ - Used under 71 | `MIT license `__ 72 | -------------------------------------------------------------------------------- /MANIFEST.in: -------------------------------------------------------------------------------- 1 | include *.rst 2 | include *.txt 3 | recursive-include abr_control *.cpp 4 | recursive-include abr_control *.dll 5 | recursive-include abr_control *.dylib 6 | recursive-include abr_control *.h 7 | recursive-include abr_control *.msim 8 | recursive-include abr_control *.py 9 | recursive-include abr_control *.pyx 10 | recursive-include abr_control *.so 11 | recursive-include abr_control *.ttt 12 | recursive-include examples *.py 13 | -------------------------------------------------------------------------------- /abr_control/__init__.py: -------------------------------------------------------------------------------- 1 | from . import controllers 2 | from .version import version as __version__ 3 | -------------------------------------------------------------------------------- /abr_control/_vendor/README.rst: -------------------------------------------------------------------------------- 1 | *********************** 2 | Vendorized dependencies 3 | *********************** 4 | 5 | This directory contains ABR_Control dependencies 6 | that have been vendorized. 7 | A vendorized dependency is shipped with ABR_Control 8 | to allow for easy offline install. 9 | 10 | To add a new vendorized dependency, 11 | add it to ``abr_control/_vendor/requirements.txt`` and run 12 | 13 | .. code:: bash 14 | 15 | pip install --target abr_control/_vendor -r abr_control/_vendor/requirements.txt 16 | 17 | from the ABR_Control root directory. 18 | 19 | To update a vendorized dependency, 20 | change the version number associated with that package 21 | in ``abr_control/_vendor/requirements.txt`` 22 | and rerun the above command 23 | from the ABR_Control root directory. 24 | -------------------------------------------------------------------------------- /abr_control/_vendor/__init__.py: -------------------------------------------------------------------------------- 1 | from . import nengolib 2 | -------------------------------------------------------------------------------- /abr_control/_vendor/nengolib/__init__.py: -------------------------------------------------------------------------------- 1 | from . import stats 2 | -------------------------------------------------------------------------------- /abr_control/_vendor/nengolib/stats/__init__.py: -------------------------------------------------------------------------------- 1 | from .ntmdists import * 2 | -------------------------------------------------------------------------------- /abr_control/_vendor/nengolib/stats/ortho.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from nengo.dists import UniformHypersphere 3 | from scipy.linalg import svd 4 | 5 | 6 | def random_orthogonal(d, rng=None): 7 | """Returns a random orthogonal matrix. 8 | 9 | Parameters 10 | ---------- 11 | d : ``integer`` 12 | Positive dimension of returned matrix. 13 | rng : :class:`numpy.random.RandomState` or ``None``, optional 14 | Random number generator state. 15 | 16 | Returns 17 | ------- 18 | samples : ``(d, d) np.array`` 19 | Random orthogonal matrix (an orthonormal basis); 20 | linearly transforms any vector into a uniformly sampled 21 | vector on the ``d``--ball with the same L2 norm. 22 | 23 | See Also 24 | -------- 25 | :class:`.ScatteredHypersphere` 26 | 27 | Examples 28 | -------- 29 | >>> from nengolib.stats import random_orthogonal, sphere 30 | >>> rng = np.random.RandomState(seed=0) 31 | >>> u = sphere.sample(1000, 3, rng=rng) 32 | >>> u[:, 0] = 0 33 | >>> v = u.dot(random_orthogonal(3, rng=rng)) 34 | 35 | >>> import matplotlib.pyplot as plt 36 | >>> from mpl_toolkits.mplot3d import Axes3D 37 | >>> ax = plt.subplot(111, projection='3d') 38 | >>> ax.scatter(*u.T, alpha=.5, label="u") 39 | >>> ax.scatter(*v.T, alpha=.5, label="v") 40 | >>> ax.patch.set_facecolor('white') 41 | >>> ax.set_xlim3d(-1, 1) 42 | >>> ax.set_ylim3d(-1, 1) 43 | >>> ax.set_zlim3d(-1, 1) 44 | >>> plt.legend() 45 | >>> plt.show() 46 | """ 47 | 48 | rng = np.random if rng is None else rng 49 | m = UniformHypersphere(surface=True).sample(d, d, rng=rng) 50 | u, s, v = svd(m) 51 | return np.dot(u, v) 52 | -------------------------------------------------------------------------------- /abr_control/_vendor/requirements.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abr/abr_control/0f6e47641d9299422fc24e09ee1445877c235cdc/abr_control/_vendor/requirements.txt -------------------------------------------------------------------------------- /abr_control/arms/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abr/abr_control/0f6e47641d9299422fc24e09ee1445877c235cdc/abr_control/arms/__init__.py -------------------------------------------------------------------------------- /abr_control/arms/jaco2/__init__.py: -------------------------------------------------------------------------------- 1 | from .config import Config 2 | -------------------------------------------------------------------------------- /abr_control/arms/onejoint/__init__.py: -------------------------------------------------------------------------------- 1 | from .config import Config 2 | -------------------------------------------------------------------------------- /abr_control/arms/onejoint/balljoint.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /abr_control/arms/onejoint/config.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import sympy as sp 3 | 4 | from ..base_config import BaseConfig 5 | 6 | 7 | class Config(BaseConfig): 8 | """Robot config file for the onelink arm 9 | 10 | Attributes 11 | ---------- 12 | START_ANGLES : numpy.array 13 | The joint angles for a safe home or rest position 14 | _M_LINKS : sympy.diag 15 | inertia matrix of the links 16 | _M_JOINTS : sympy.diag 17 | inertia matrix of the joints 18 | L : numpy.array 19 | segment lengths of arm [meters] 20 | 21 | Transform Naming Convention: Tpoint1point2 22 | ex: Tj1l1 transforms from joint 1 to link 1 23 | 24 | Transforms are broken up into two matrices for simplification 25 | ex: Tj0l1a and Tj0l1b where the former transform accounts for 26 | joint rotations and the latter accounts for static rotations 27 | and translations 28 | """ 29 | 30 | def __init__(self, **kwargs): 31 | 32 | super().__init__(N_JOINTS=1, N_LINKS=1, ROBOT_NAME="onelink", **kwargs) 33 | 34 | self._T = {} # dictionary for storing calculated transforms 35 | 36 | self.JOINT_NAMES = ["joint0"] 37 | self.START_ANGLES = np.array([np.pi / 2.0]) 38 | 39 | # create the inertia matrices for each link 40 | self._M_LINKS.append(np.diag([1.0, 1.0, 1.0, 0.02, 0.02, 0.02])) # link0 41 | 42 | # the joints don't weigh anything 43 | self._M_JOINTS = [sp.zeros(6, 6) for ii in range(self.N_JOINTS)] 44 | 45 | # segment lengths associated with each joint 46 | self.L = np.array( 47 | [ 48 | [0.0, 0.0, 0.05], # from origin to l0 COM 49 | [0.0, 0.0, 0.05], # from l0 COM to j0 50 | [0.22, 0.0, 0.0], # from j0 to l1 COM 51 | [0.0, 0.0, 0.15], 52 | ] 53 | ) # from l1 COM to EE 54 | 55 | # ---- Transform Matrices ---- 56 | 57 | # Transform matrix : origin -> link 0 58 | # account for axes change and offsets 59 | self.Torgl0 = sp.Matrix( 60 | [ 61 | [1, 0, 0, self.L[0, 0]], 62 | [0, 1, 0, self.L[0, 1]], 63 | [0, 0, 1, self.L[0, 2]], 64 | [0, 0, 0, 1], 65 | ] 66 | ) 67 | 68 | # Transform matrix : link 0 -> joint 0 69 | # account for axes change and offsets 70 | self.Tl0j0 = sp.Matrix( 71 | [ 72 | [1, 0, 0, self.L[1, 0]], 73 | [0, 0, -1, self.L[1, 1]], 74 | [0, 1, 0, self.L[1, 2]], 75 | [0, 0, 0, 1], 76 | ] 77 | ) 78 | 79 | # Transform matrix : joint 0 -> link 1 80 | # account for rotation due to q 81 | self.Tj0l1a = sp.Matrix( 82 | [ 83 | [sp.cos(self.q[0]), -sp.sin(self.q[0]), 0, 0], 84 | [sp.sin(self.q[0]), sp.cos(self.q[0]), 0, 0], 85 | [0, 0, 1, 0], 86 | [0, 0, 0, 1], 87 | ] 88 | ) 89 | # account for change of axes and offsets 90 | self.Tj0l1b = sp.Matrix( 91 | [ 92 | [0, 0, 1, self.L[2, 0]], 93 | [0, 1, 0, self.L[2, 1]], 94 | [-1, 0, 0, self.L[2, 2]], 95 | [0, 0, 0, 1], 96 | ] 97 | ) 98 | self.Tj0l1 = self.Tj0l1a * self.Tj0l1b 99 | 100 | # Transform matrix : link 1 -> end-effector 101 | self.Tl1ee = sp.Matrix( 102 | [ 103 | [1, 0, 0, self.L[3, 0]], 104 | [0, 1, 0, self.L[3, 1]], 105 | [0, 0, 1, self.L[3, 2]], 106 | [0, 0, 0, 1], 107 | ] 108 | ) 109 | 110 | # orientation part of the Jacobian (compensating for angular velocity) 111 | self.J_orientation = [ 112 | self._calc_T("joint0")[:3, :3] * self._KZ 113 | ] # joint 0 orientation 114 | 115 | def _calc_T(self, name): # noqa C907 116 | """Uses Sympy to generate the transform for a joint or link 117 | 118 | name : string 119 | name of the joint, link, or end-effector 120 | """ 121 | 122 | if self._T.get(name, None) is None: 123 | if name == "link0": 124 | self._T[name] = self.Torgl0 125 | elif name == "joint0": 126 | self._T[name] = self._calc_T("link0") * self.Tl0j0 127 | elif name == "link1": 128 | self._T[name] = self._calc_T("joint0") * self.Tj0l1 129 | elif name == "EE": 130 | self._T[name] = self._calc_T("link1") * self.Tl1ee 131 | 132 | else: 133 | raise Exception(f"Invalid transformation name: {name}") 134 | 135 | return self._T[name] 136 | -------------------------------------------------------------------------------- /abr_control/arms/onejoint/onejoint.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /abr_control/arms/tests/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abr/abr_control/0f6e47641d9299422fc24e09ee1445877c235cdc/abr_control/arms/tests/__init__.py -------------------------------------------------------------------------------- /abr_control/arms/threejoint/__init__.py: -------------------------------------------------------------------------------- 1 | from .arm_sim import ArmSim 2 | from .config import Config 3 | -------------------------------------------------------------------------------- /abr_control/arms/threejoint/arm_files/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abr/abr_control/0f6e47641d9299422fc24e09ee1445877c235cdc/abr_control/arms/threejoint/arm_files/__init__.py -------------------------------------------------------------------------------- /abr_control/arms/threejoint/arm_files/arm3link.msim: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abr/abr_control/0f6e47641d9299422fc24e09ee1445877c235cdc/abr_control/arms/threejoint/arm_files/arm3link.msim -------------------------------------------------------------------------------- /abr_control/arms/threejoint/arm_files/py3LinkArm.pyx: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | cimport numpy as np 4 | 5 | 6 | cdef extern from "threelinkarm.cpp": 7 | cdef cppclass Sim: 8 | Sim(double dt, double* params) 9 | void reset(double* out, double* ic) 10 | void step(double* out, double* u) 11 | 12 | cdef class pySim: 13 | cdef Sim* thisptr 14 | def __cinit__(self, double dt = .00001, 15 | np.ndarray[double, mode="c"] params=None): 16 | """ 17 | param float dt: simulation timestep, must be < 1e-5 18 | param array params: MapleSim model internal parameters 19 | """ 20 | if params: self.thisptr = new Sim(dt, ¶ms[0]) 21 | else: self.thisptr = new Sim(dt, NULL) 22 | 23 | def __dealloc__(self): 24 | del self.thisptr 25 | 26 | def reset(self, np.ndarray[double, mode="c"] out, 27 | np.ndarray[double, mode="c"] ic=None): 28 | """ 29 | Reset the state of the simulation. 30 | param np.ndarray out: where to store the system output 31 | NOTE: output is of form [time, output] 32 | param np.ndarray ic: the initial conditions of the system 33 | """ 34 | if ic is not None: self.thisptr.reset(&out[0], &ic[0]) 35 | else: self.thisptr.reset(&out[0], NULL) 36 | 37 | def step(self, np.ndarray[double, mode="c"] out, 38 | np.ndarray[double, mode="c"] u): 39 | """ 40 | Step the simulation forward one timestep. 41 | param np.ndarray out: where to store the system output 42 | NOTE: output is of form [time, output] 43 | param np.ndarray u: the control signal 44 | """ 45 | self.thisptr.step(&out[0], &u[0]) 46 | -------------------------------------------------------------------------------- /abr_control/arms/threejoint/arm_files/py3LinkArm.pyxbld: -------------------------------------------------------------------------------- 1 | import numpy 2 | import pathlib 3 | 4 | path = str(pathlib.Path(__file__).parent.absolute()) 5 | 6 | def make_ext(modname, pyxfilename): 7 | from distutils.extension import Extension 8 | return Extension(name=modname, 9 | sources=[pyxfilename], 10 | language='c++', 11 | include_dirs=[numpy.get_include()], 12 | extra_compile_args=['-I' + path],) 13 | -------------------------------------------------------------------------------- /abr_control/arms/threejoint/arm_sim.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pyximport 3 | 4 | pyximport.install(inplace=True) 5 | 6 | from .arm_files.py3LinkArm import pySim # pylint: disable=C0413 7 | 8 | 9 | class ArmSim: 10 | """An interface for the three-link MapleSim model 11 | 12 | An interface for the three-link MapleSim model that has been exported 13 | to C and turned into shared libraries using Cython. 14 | 15 | Parameters 16 | ---------- 17 | robot_config : class instance 18 | contains all relevant information about the arm 19 | such as: number of joints, number of links, mass information etc. 20 | dt: float, optional (Default: 0.001) 21 | simulation time step [seconds] 22 | q_init : numpy.array, optional (Default: robot_config.START_ANGLES) 23 | start joint angles [radians] 24 | dq_init : numpy.array, optional (Default: np.zeros) 25 | start joint velocity [radians/second] 26 | """ 27 | 28 | def __init__(self, robot_config, dt=0.001, q_init=None, dq_init=None): 29 | 30 | self.robot_config = robot_config 31 | 32 | # create placeholders for joint angles and velocity 33 | self.q = np.zeros(self.robot_config.N_JOINTS) 34 | self.dq = np.zeros(self.robot_config.N_JOINTS) 35 | 36 | self.init_state = np.zeros(self.robot_config.N_JOINTS * 2) 37 | if q_init is None: 38 | self.init_state[::2] = self.robot_config.START_ANGLES 39 | else: 40 | self.init_state[::2] = q_init 41 | if dq_init is not None: 42 | self.init_state[1::2] = dq_init 43 | 44 | self.dt = dt # time step 45 | 46 | self.torque_limit = 1e7 # max amplitude of torque signal allowed 47 | self.connect() 48 | 49 | def connect(self): 50 | """Creates the MapleSim model and set up PyGame.""" 51 | 52 | # stores information returned from maplesim 53 | self.state = np.zeros(7) 54 | self.sim = pySim(dt=1e-5) 55 | 56 | self.reset() 57 | print("Connected to MapleSim model") 58 | 59 | def disconnect(self): 60 | """Reset the simulation and close PyGame display.""" 61 | 62 | self.reset() 63 | print("MapleSim connection closed...") 64 | 65 | def reset(self): 66 | """Resets the state of the arm to starting conditions.""" 67 | 68 | self.sim.reset(self.state, self.init_state) 69 | self._update_state() 70 | 71 | def send_forces(self, u, dt=None): 72 | """Apply the specified forces to the robot, 73 | moving the simulation one time step forward. 74 | 75 | NOTE: For this simulation, torques are clipped to 1e7 76 | to prevent seg faults being thrown. 77 | 78 | Parameters 79 | ---------- 80 | u : numpy.array 81 | an array of the torques to apply to the robot [Nm] 82 | dt : float, optional (Default: self.dt) 83 | time step [seconds] 84 | """ 85 | 86 | dt = self.dt if dt is None else dt 87 | # clip the torque signal to prevent seg faults 88 | u = np.minimum( 89 | np.maximum(-1 * np.array(u, dtype="float"), -self.torque_limit), 90 | self.torque_limit, 91 | ) 92 | 93 | for _ in range(int(np.ceil(dt / 1e-5))): 94 | self.sim.step(self.state, u) 95 | self._update_state() 96 | 97 | def get_feedback(self): 98 | """Return a dictionary of information needed by the controller.""" 99 | 100 | return {"q": self.q, "dq": self.dq} 101 | 102 | def get_xyz(self, name): 103 | """Not available in the MapleSim Interface""" 104 | 105 | raise NotImplementedError( 106 | "Not an available method" + "in the MapleSim interface" 107 | ) 108 | 109 | def _position(self): 110 | """Compute x,y position of the hand""" 111 | 112 | xy = [ 113 | self.robot_config.Tx(f"joint{ii}", q=self.q) 114 | for ii in range(self.robot_config.N_JOINTS) 115 | ] 116 | xy = np.vstack([xy, self.robot_config.Tx("EE", q=self.q)]) 117 | self.joints_x = xy[:, 0] 118 | self.joints_y = xy[:, 1] 119 | return np.array([self.joints_x, self.joints_y]) 120 | 121 | def _update_state(self): 122 | """Update the local variables""" 123 | 124 | self.t = self.state[0] 125 | self.q = self.state[1:4] 126 | self.dq = self.state[4:] 127 | self._position() 128 | self.x = np.array([self.joints_x[-1], self.joints_y[-1]]) 129 | -------------------------------------------------------------------------------- /abr_control/arms/threejoint/threejoint.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /abr_control/arms/twojoint/__init__.py: -------------------------------------------------------------------------------- 1 | from .arm_sim import ArmSim 2 | from .config import Config 3 | -------------------------------------------------------------------------------- /abr_control/arms/twojoint/arm_sim.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class ArmSim: 5 | """An interface for a Python implementation of a 2 link arm. 6 | 7 | An interface for the two-link Python model arm. 8 | 9 | Parameters 10 | ---------- 11 | robot_config : class instance 12 | contains all relevant information about the arm 13 | such as: number of joints, number of links, mass information etc. 14 | dt: float, optional (Default: 0.001) 15 | simulation time step [seconds] 16 | q_init : numpy.array, optional (Default: robot_config.START_ANGLES) 17 | start joint angles [radians] 18 | """ 19 | 20 | def __init__(self, robot_config, dt=0.001, q_init=None): 21 | 22 | self.robot_config = robot_config 23 | 24 | self.q_init = q_init if q_init is not None else self.robot_config.START_ANGLES 25 | self.reset() 26 | 27 | M = self.robot_config._M_LINKS 28 | L = [] 29 | for ii in range(int(self.robot_config.L.shape[0] / 2)): 30 | L.append(np.sum(self.robot_config.L[ii * 2 : ii * 2 + 2])) 31 | 32 | # compute non changing constants 33 | self.K1 = (1 / 3.0 * M[1][0, 0] + M[2][0, 0]) * L[1] ** 2.0 + 1 / 3.0 * M[2][ 34 | 0, 0 35 | ] * L[2] ** 2.0 36 | self.K2 = M[2][0, 0] * L[1] * L[2] 37 | self.K3 = 1 / 3.0 * M[2][0, 0] * L[2] ** 2.0 38 | self.K4 = 1 / 2.0 * M[2][0, 0] * L[1] * L[2] 39 | 40 | self.dt = dt # time step 41 | self.t = 0.0 # time 42 | 43 | def connect(self): 44 | """Reset the state of the system.""" 45 | self.reset() 46 | self._update_state() 47 | # NOTE: This is kind of a meaningless comment, not sure if worth having 48 | print("Connected to Python model") 49 | 50 | def disconnect(self): 51 | """Reset the simulation and close PyGame display.""" 52 | 53 | self.reset() 54 | self._update_state() 55 | # NOTE: This is kind of a meaningless comment, not sure if worth having 56 | print("Python model connection closed...") 57 | 58 | def get_feedback(self): 59 | """Return a dictionary of information needed by the controller.""" 60 | 61 | return {"q": self.q, "dq": self.dq} 62 | 63 | def get_xyz(self, name): 64 | raise NotImplementedError( 65 | "Not an available method" + "in the MapleSim interface" 66 | ) 67 | 68 | def send_forces(self, u, dt=None): 69 | """Apply the specified forces to the robot, 70 | move the simulation one time step forward, and update 71 | the plot. 72 | 73 | Parameters 74 | ---------- 75 | u : numpy.array 76 | an array of the torques to apply to the robot [Nm] 77 | dt : float, optional (Default: None) 78 | time step [seconds] 79 | """ 80 | 81 | self._step(u, dt) 82 | 83 | def reset(self): 84 | """Resets the state of the arm to starting conditions.""" 85 | 86 | self.q = np.copy(self.q_init) 87 | self.dq = np.zeros(self.q.shape) 88 | 89 | def _position(self): 90 | """Compute x,y position of the hand""" 91 | 92 | xy = [ 93 | self.robot_config.Tx(f"joint{ii}", q=self.q) 94 | for ii in range(self.robot_config.N_JOINTS) 95 | ] 96 | xy = np.vstack([xy, self.robot_config.Tx("EE", q=self.q)]) 97 | self.joints_x = xy[:, 0] 98 | self.joints_y = xy[:, 1] 99 | return np.array([self.joints_x, self.joints_y]) 100 | 101 | def _step(self, u, dt=None): 102 | """Simulate the system one time step 103 | 104 | Parameters 105 | ---------- 106 | u : numpy.array 107 | an array of the torques to apply to the robot 108 | dt : float, optional (Default: self.dt) 109 | time step [seconds] 110 | """ 111 | 112 | dt = self.dt if dt is None else dt 113 | 114 | # equations solved for angles 115 | C2 = np.cos(self.q[1]) 116 | S2 = np.sin(self.q[1]) 117 | M11 = self.K1 + self.K2 * C2 118 | M12 = self.K3 + self.K4 * C2 119 | M21 = M12 120 | M22 = self.K3 121 | H1 = ( 122 | -self.K2 * S2 * self.dq[0] * self.dq[1] 123 | - 1.0 / 2.0 * self.K2 * S2 * self.dq[1] ** 2.0 124 | ) 125 | H2 = 1.0 / 2.0 * self.K2 * S2 * self.dq[0] ** 2.0 126 | 127 | ddq1 = (H2 * M11 - H1 * M21 - M11 * u[1] + M21 * u[0]) / ( 128 | M12 ** 2.0 - M11 * M22 129 | ) 130 | ddq0 = (-H2 + u[1] - M22 * ddq1) / M21 131 | self.dq += np.array([ddq0, ddq1]) * dt 132 | self.q += self.dq * dt 133 | 134 | # transfer to next time step 135 | self.t += self.dt 136 | 137 | self._update_state() 138 | 139 | def _update_state(self): 140 | """Update local variables""" 141 | 142 | self._position() 143 | self.x = np.array([self.joints_x[-1], self.joints_y[-1]]) 144 | -------------------------------------------------------------------------------- /abr_control/arms/twojoint/twojoint.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /abr_control/arms/ur5/__init__.py: -------------------------------------------------------------------------------- 1 | from .config import Config 2 | -------------------------------------------------------------------------------- /abr_control/controllers/__init__.py: -------------------------------------------------------------------------------- 1 | from .avoid_joint_limits import AvoidJointLimits 2 | from .avoid_obstacles import AvoidObstacles 3 | from .damping import Damping 4 | from .floating import Floating 5 | from .joint import Joint 6 | from .osc import OSC 7 | from .resting_config import RestingConfig 8 | from .sliding import Sliding 9 | -------------------------------------------------------------------------------- /abr_control/controllers/controller.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class Controller: 5 | """ 6 | The base functions for all controllers 7 | 8 | Parameters 9 | ---------- 10 | robot_config : class instance 11 | contains all relevant information about the arm 12 | such as: number of joints, number of links, mass information etc. 13 | """ 14 | 15 | def __init__(self, robot_config): 16 | self.robot_config = robot_config 17 | 18 | self.offset_zeros = np.zeros(3) 19 | 20 | def generate(self, q, dq): 21 | """ 22 | Generate the torques to apply to robot joints 23 | 24 | Parameters 25 | ---------- 26 | q : float numpy.array 27 | joint angles [radians] 28 | dq : float numpy.array 29 | the current joint velocities [radians/second] 30 | 31 | """ 32 | raise NotImplementedError 33 | -------------------------------------------------------------------------------- /abr_control/controllers/damping.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from .controller import Controller 4 | 5 | 6 | class Damping(Controller): 7 | """Base class for common null space controllers. 8 | 9 | Parameters 10 | ---------- 11 | robot_config : class instance 12 | contains all relevant information about the arm 13 | such as: number of joints, number of links, mass information etc. 14 | """ 15 | 16 | def __init__(self, robot_config, kv): 17 | super().__init__(robot_config) 18 | 19 | self.kv = kv 20 | 21 | def generate(self, q, dq): 22 | """Generates the control signal 23 | 24 | q : np.array 25 | the current joint angles [radians] 26 | dq : np.array 27 | the current joint angle velocity [radians/second] 28 | """ 29 | 30 | # calculate joint space inertia matrix 31 | M = self.robot_config.M(q=q) 32 | return np.dot(M, -self.kv * dq) 33 | -------------------------------------------------------------------------------- /abr_control/controllers/floating.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from .controller import Controller 4 | 5 | 6 | class Floating(Controller): 7 | """Implements a controller to compensate for gravity 8 | Only compensates for the effects of gravity on the arm. The arm will 9 | remain compliant and hold whatever position it is placed in (as long 10 | as an accurate mass / inertia model is provided) 11 | Parameters 12 | ---------- 13 | robot_config: class instance 14 | contains all relevant information about the arm 15 | such as: number of joints, number of links, mass information etc. 16 | task_space: boolean, optional (Default: False) 17 | if True, perform the calculation to cancel out the effects of 18 | gravity in task-space 19 | """ 20 | 21 | def __init__(self, robot_config, dynamic=False, task_space=False): 22 | super().__init__(robot_config) 23 | 24 | self.dynamic = dynamic 25 | self.task_space = task_space 26 | 27 | def generate(self, q, dq=None): 28 | """Generates the control signal to compensate for gravity 29 | Parameters 30 | ---------- 31 | q : float numpy.array 32 | the current joint angles [radians] 33 | dq : float numpy.array 34 | the current joint velocities [radians/second] 35 | """ 36 | # calculate the effect of gravity in joint space 37 | g = self.robot_config.g(q) 38 | 39 | if self.task_space: 40 | # get the Jacobian 41 | J = self.robot_config.J("EE", q)[:3] 42 | 43 | # calculate the inertia matrix in joint space 44 | M = self.robot_config.M(q) 45 | # calculate the inertia matrix in task space 46 | M_inv = np.linalg.inv(M) 47 | 48 | Mx_inv = np.dot(J, np.dot(M_inv, J.T)) 49 | if abs(np.linalg.det(Mx_inv)) > 1e-3: 50 | Mx = np.linalg.inv(Mx_inv) 51 | else: 52 | # using the rcond to set singular values < thresh to 0 53 | # is slightly faster than doing it manually with svd 54 | # singular values < (rcond * max(singular_values)) set to 0 55 | Mx = np.linalg.pinv(Mx_inv, rcond=1e-4) 56 | 57 | # calculate the effect of gravity in joint space 58 | Jbar = np.dot(M_inv, np.dot(J.T, Mx)) 59 | u_task = -1 * np.dot(Jbar.T, g) 60 | u = np.dot(J.T, u_task) 61 | else: 62 | # generate the control signal in joint space 63 | u = -g 64 | M = None 65 | 66 | if self.dynamic: 67 | # compensate for current velocity 68 | M = self.robot_config.M(q) if M is None else M 69 | u -= np.dot(M, dq) 70 | 71 | return u 72 | -------------------------------------------------------------------------------- /abr_control/controllers/joint.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from abr_control.utils.transformations import quaternion_conjugate, quaternion_multiply 4 | 5 | from .controller import Controller 6 | 7 | 8 | class Joint(Controller): 9 | """Implements a joint space controller 10 | 11 | Moves the arm joints to a set of specified target angles 12 | 13 | Parameters 14 | ---------- 15 | robot_config : class instance 16 | contains all relevant information about the arm 17 | such as: number of joints, number of links, mass information etc. 18 | kp : float, optional (Default: 1) 19 | proportional gain term 20 | kv : float, optional (Default: None) 21 | derivative gain term, a good starting point is sqrt(kp) 22 | quaternions : list, optional (Default: None) 23 | a boolean list of which joints are quaternions 24 | """ 25 | 26 | def __init__( 27 | self, robot_config, kp=1, kv=None, quaternions=None, account_for_gravity=True 28 | ): 29 | super().__init__(robot_config) 30 | 31 | self.kp = kp 32 | self.kv = np.sqrt(self.kp) if kv is None else kv 33 | self.account_for_gravity = account_for_gravity 34 | self.ZEROS_N_JOINTS = np.zeros(robot_config.N_JOINTS) 35 | 36 | if quaternions is not None: 37 | self.quaternions = quaternions 38 | self.q_tilde = self.q_tilde_quat 39 | else: 40 | self.q_tilde = self.q_tilde_angle 41 | 42 | def q_tilde_angle(self, q, target): 43 | # calculate the direction for each joint to move, wrapping 44 | # around the -pi to pi limits to find the shortest distance 45 | q_tilde = ((target - q + np.pi) % (np.pi * 2)) - np.pi 46 | return q_tilde 47 | 48 | def q_tilde_quat(self, q, target): 49 | """Compute the error signal when there are quaternions in the state 50 | space and target signals. If there are quaternions in the state space, 51 | calculate the error and then transform them to 3D space for the control 52 | signal. 53 | 54 | NOTE: Assumes that for every quaternion there are 3 motors, that effect 55 | movement along the x, y, and z axes, applied in that order. 56 | 57 | Parameters 58 | ---------- 59 | q : float numpy.array 60 | mix of joint angles and quaternions [quaternions & radians] 61 | target : float numpy.array 62 | mix of target joint angles and quaternions [quaternions & radians] 63 | """ 64 | # for each quaternion in the list, the output size is reduced by 1 65 | q_tilde = np.zeros(len(q) - np.sum(self.quaternions)) 66 | 67 | q_index = 0 68 | q_tilde_index = 0 69 | 70 | for quat_bool in self.quaternions: 71 | if quat_bool: 72 | 73 | # if the joint position is a quaternion, calculate error 74 | joint_q = q[q_index : q_index + 4] 75 | error = quaternion_multiply( 76 | target[q_index : q_index + 4], 77 | quaternion_conjugate(joint_q), 78 | ) 79 | 80 | # NOTE: we need to rotate this back to undo the current angle 81 | # because the actuators don't rotate with the ball joint 82 | u = quaternion_multiply( 83 | quaternion_conjugate(joint_q), 84 | quaternion_multiply( 85 | error, 86 | joint_q, 87 | ), 88 | ) 89 | 90 | # convert from quaternion to 3D angular forces to apply 91 | # https://studywolf.wordpress.com/2018/12/03/force-control-of-task-space-orientation/ 92 | q_tilde[q_tilde_index : q_tilde_index + 3] = u[1:] * np.sign(u[0]) 93 | 94 | q_index += 4 95 | q_tilde_index += 3 96 | else: 97 | q_tilde[q_tilde_index] = self.q_tilde_angle(q[q_index], target[q_index]) 98 | 99 | q_index += 1 100 | q_tilde_index += 1 101 | 102 | return q_tilde 103 | 104 | def generate(self, q, dq, target, target_velocity=None): 105 | """Generate a joint space control signal 106 | 107 | Parameters 108 | ---------- 109 | q : float numpy.array 110 | current joint angles [radians] 111 | dq : float numpy.array 112 | current joint velocities [radians/second] 113 | target : float numpy.array 114 | desired joint angles [radians] 115 | target_velocity : float numpy.array, optional (Default: None) 116 | desired joint velocities [radians/sec] 117 | """ 118 | 119 | if target_velocity is None: 120 | target_velocity = self.ZEROS_N_JOINTS 121 | 122 | q_tilde = self.q_tilde(q, target) 123 | 124 | # get the joint space inertia matrix 125 | M = self.robot_config.M(q) 126 | u = np.dot(M, (self.kp * q_tilde + self.kv * (target_velocity - dq))) 127 | # account for gravity 128 | if self.account_for_gravity: 129 | u -= self.robot_config.g(q) 130 | 131 | return u 132 | -------------------------------------------------------------------------------- /abr_control/controllers/path_planners/__init__.py: -------------------------------------------------------------------------------- 1 | from .inverse_kinematics import InverseKinematics 2 | from .orientation import Orientation 3 | from .path_planner import PathPlanner 4 | -------------------------------------------------------------------------------- /abr_control/controllers/path_planners/velocity_profiles.py: -------------------------------------------------------------------------------- 1 | """ 2 | Functions that return a 1D array of velocities from a desired start velocity to 3 | a target velocity. 4 | """ 5 | import numpy as np 6 | 7 | 8 | class VelProf: 9 | def __init__(self, dt): 10 | """ 11 | Must take the sim timestep on init, as the path_planner requires it. 12 | """ 13 | self.dt = dt 14 | 15 | def generate(self, start_velocity, target_velocity): 16 | """ 17 | Takes start and target velocities as a float, and returns a 1xN array 18 | of velocities that go from start to target. 19 | """ 20 | raise NotImplementedError 21 | 22 | 23 | class Gaussian(VelProf): 24 | def __init__(self, dt, acceleration, n_sigma=3): 25 | """ 26 | Velocity profile that follows a gaussian curve. 27 | 28 | Parameters 29 | ---------- 30 | dt: float 31 | The timestep (in seconds). 32 | acceleration: float 33 | The acceleration that defines our velocity curve. 34 | n_sigma: int, Optional (Default: 3) 35 | How many standard deviations of the gaussian function to use for the 36 | velocity profile. The default value of 3 gives a smooth acceleration and 37 | deceleration. A slower ramp up can be achieved with a larger sigma, and a 38 | faster ramp up by decreasing sigma. Note that the curve gets shifted so 39 | that it starts at zero, since the gaussian has a domain of [-inf, inf]. 40 | At sigma==3 we get close to zero and have a smooth ramp up. 41 | """ 42 | self.acceleration = acceleration 43 | self.n_sigma = n_sigma 44 | 45 | super().__init__(dt=dt) 46 | 47 | def generate(self, start_velocity, target_velocity): 48 | """ 49 | Generates the left half of the gaussian curve with n_sigma std, with 50 | sigma determined by the timestep and max_a. 51 | 52 | Parameters 53 | ---------- 54 | start_velocity: float 55 | The starting velocity in the curve. 56 | target_velocity: float 57 | The ending velocity in the curve. 58 | """ 59 | # calculate the time needed to reach our maximum velocity from vel 60 | ramp_up_time = (target_velocity - start_velocity) / self.acceleration 61 | 62 | # Amplitude of Gaussian is max speed, get our sigma from here 63 | s = 1 / ((target_velocity - start_velocity) * np.sqrt(np.pi * 2)) 64 | 65 | # Go 3 standard deviations so our tail ends get close to zero 66 | u = self.n_sigma * s 67 | 68 | # We are generating the left half of the gaussian, so generate our 69 | # x values from 0 to the mean, with the steps determined by our 70 | # ramp up time (which itself is determined by max_a) 71 | x = np.linspace(0, u, int(ramp_up_time / self.dt)) 72 | 73 | # Get our gaussian y values, which is our normalized velocity profile 74 | vel_profile = 1 * ( 75 | 1 / (s * np.sqrt(2 * np.pi)) * np.exp(-0.5 * ((x - u) / s) ** 2) 76 | ) 77 | 78 | # Since the gaussian goes off to +/- infinity, we need to shift our 79 | # curve down so that it start_velocitys at zero 80 | vel_profile -= vel_profile[0] 81 | 82 | # scale back up so we reach our target v at the end of the curve 83 | vel_profile *= (target_velocity - start_velocity) / vel_profile[-1] 84 | 85 | # add to our baseline starting velocity 86 | vel_profile += start_velocity 87 | 88 | return vel_profile 89 | 90 | 91 | class Linear(VelProf): 92 | def __init__(self, dt, acceleration): 93 | """ 94 | Velocity profile that follows a linear curve. 95 | 96 | Parameters 97 | ---------- 98 | dt: float 99 | The timestep (in seconds). 100 | acceleration: float 101 | The acceleration that defines the velocity curve. 102 | """ 103 | self.acceleration = acceleration 104 | 105 | super().__init__(dt=dt) 106 | 107 | def generate(self, start_velocity, target_velocity): 108 | """ 109 | Generates a linear ramp from start to target velocity, with a slope of 110 | self.acceleration. 111 | 112 | Parameters 113 | ---------- 114 | start_velocity: float 115 | The starting velocity in the curve. 116 | target_velocity: float 117 | The ending velocity in the curve. 118 | """ 119 | 120 | vdiff = target_velocity - start_velocity 121 | t = vdiff / self.acceleration 122 | steps = t / self.dt 123 | vel_profile = np.linspace(start_velocity, target_velocity, int(steps)) 124 | 125 | return vel_profile 126 | -------------------------------------------------------------------------------- /abr_control/controllers/resting_config.py: -------------------------------------------------------------------------------- 1 | # TODO: set up to work with ball joints / quaternion positions 2 | 3 | import numpy as np 4 | 5 | from .joint import Joint 6 | 7 | 8 | class RestingConfig(Joint): 9 | """Move the arm towards a set of 'resting state' joint angles 10 | 11 | Parameters 12 | ---------- 13 | robot_config: class instance 14 | contains all relevant information about the arm 15 | such as number of joints, number of links, mass information etc. 16 | """ 17 | 18 | def __init__(self, robot_config, rest_angles, **kwargs): 19 | super().__init__(robot_config, account_for_gravity=False, **kwargs) 20 | 21 | self.rest_angles = np.asarray(rest_angles) 22 | self.rest_indices = [val is not None for val in rest_angles] 23 | self.q_tilde = self.q_tilde_angle 24 | 25 | def q_tilde_angle(self, q, target): 26 | # distance / direction 27 | q_tilde = np.zeros(len(q)) 28 | q_tilde[self.rest_indices] = ( 29 | self.rest_angles[self.rest_indices] - q[self.rest_indices] + np.pi 30 | ) % (np.pi * 2) - np.pi 31 | return q_tilde 32 | 33 | def generate(self, q, dq): 34 | """Generates the control signal 35 | 36 | q: np.array 37 | the current joint angles [radians] 38 | dq: np.array 39 | the current joint angle velocity [radians/second] 40 | """ 41 | 42 | return super().generate(q, dq, target=self.rest_angles) 43 | -------------------------------------------------------------------------------- /abr_control/controllers/signals/__init__.py: -------------------------------------------------------------------------------- 1 | from .dynamics_adaptation import DynamicsAdaptation 2 | -------------------------------------------------------------------------------- /abr_control/controllers/signals/tests/test_dynamics_adaptation.py: -------------------------------------------------------------------------------- 1 | import nengo 2 | import numpy as np 3 | import pytest 4 | 5 | from abr_control._vendor.nengolib.stats import ScatteredHypersphere 6 | from abr_control.arms import ur5 7 | from abr_control.controllers import signals 8 | from abr_control.controllers.signals import DynamicsAdaptation 9 | 10 | 11 | def test_scaling(): 12 | robot_config = ur5.Config(use_cython=True) 13 | 14 | # looking at data centered around 50 with range of 25 each way 15 | means = np.ones(robot_config.N_JOINTS) * 50 16 | variances = np.ones(robot_config.N_JOINTS) * 25 17 | 18 | for ii in range(50): 19 | # test without spherical conversion 20 | adapt = DynamicsAdaptation( 21 | robot_config.N_JOINTS, 22 | robot_config.N_JOINTS, 23 | means=means, 24 | variances=variances, 25 | spherical=False, 26 | ) 27 | 28 | unscaled = np.random.random(robot_config.N_JOINTS) * 50 + 25 29 | scaled = adapt.scale_inputs(unscaled) 30 | assert np.all(-1 < scaled) and np.all(scaled < 1) 31 | 32 | # test spherical conversion 33 | adapt = DynamicsAdaptation( 34 | robot_config.N_JOINTS, 35 | robot_config.N_JOINTS, 36 | means=means, 37 | variances=variances, 38 | spherical=True, 39 | ) 40 | 41 | unscaled = np.random.random(robot_config.N_JOINTS) * 50 + 25 42 | scaled = adapt.scale_inputs(unscaled) 43 | assert np.linalg.norm(scaled) - 1 < 1e-5 44 | -------------------------------------------------------------------------------- /abr_control/controllers/sliding.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from .controller import Controller 4 | 5 | 6 | class Sliding(Controller): 7 | """Implements sliding control based on the description in 8 | [Slotine, Jean-Jacques E., and Weiping Li. "On the adaptive control of robot 9 | manipulators." The international journal of robotics research 6.3 (1987): 49-59.] 10 | 11 | Parameters 12 | ---------- 13 | robot_config : class instance 14 | contains all relevant information about the arm 15 | such as: number of joints, number of links, mass information etc. 16 | kd : float, optional (Default: 160) 17 | gain term 18 | lambda : float, optional (Default: 30) 19 | gain term 20 | cartesian : boolean, optional (Default: True) 21 | if True transforms control from Cartesian into joint space 22 | if False control assumed to be entirely in joint space 23 | 24 | """ 25 | 26 | def __init__(self, robot_config, kd=160.0, lamb=30.0, cartesian=True): 27 | 28 | super().__init__(robot_config) 29 | 30 | self.kd = kd 31 | self.lamb = lamb 32 | self.cartesian = cartesian 33 | 34 | def generate( 35 | self, 36 | q, 37 | dq, 38 | target, 39 | target_velocity=0, 40 | target_acc=0, 41 | ref_frame="EE", 42 | offset=None, 43 | ): 44 | """Generates the control signal to move the EE to a target 45 | 46 | Parameters 47 | ---------- 48 | q : float numpy.array 49 | current joint angles [radians] 50 | dq : float numpy.array 51 | current joint velocities [radians/second] 52 | target : float numpy.array 53 | desired joint angles [radians] 54 | target_velocity : float numpy.array, optional (Default: numpy.zeros) 55 | desired joint velocities [radians/sec] 56 | ref_frame : string, optional (Default: 'EE') 57 | the point being controlled, default is the end-effector. 58 | offset : list, optional (Default: None) 59 | point of interest inside the frame of reference [meters] 60 | """ 61 | 62 | offset = self.offset_zeros if offset is None else offset 63 | 64 | if self.cartesian: 65 | # calculate the position Jacobian for the end effector 66 | J = self.robot_config.J(ref_frame, q, x=offset)[:3] 67 | 68 | # calculate the end-effector position information 69 | xyz = self.robot_config.Tx(ref_frame, q, x=offset) 70 | dxyz = np.dot(J, dq) 71 | 72 | J_inv = np.linalg.pinv(J) 73 | dJ = self.robot_config.dJ(ref_frame, q, dq, x=offset)[:3] 74 | 75 | dq_ref = np.dot(J_inv, target_velocity + self.lamb * (target - xyz)) 76 | ddq_ref = np.dot( 77 | J_inv, 78 | target_acc + self.lamb * (target_velocity - dxyz) - np.dot(dJ, dq_ref), 79 | ) 80 | else: 81 | q_tilde = q - target 82 | dq_tilde = dq - target_velocity 83 | dq_ref = target_velocity - self.lamb * q_tilde 84 | ddq_ref = target_acc - self.lamb * dq_tilde 85 | 86 | # store the control signal s for training in case 87 | # dynamics adaptation signal is being used 88 | self.s = dq - dq_ref 89 | 90 | # calculate the inertia matrix in joint space 91 | M = self.robot_config.M(q) 92 | # calculate the partial centrifugal and Coriolis effects 93 | C = self.robot_config.C(q=q, dq=dq) 94 | # calculate the effects of gravity 95 | g = self.robot_config.g(q=q) 96 | 97 | u = np.dot(M, ddq_ref) + np.dot(C, dq_ref) + g - self.kd * self.s 98 | 99 | return u 100 | -------------------------------------------------------------------------------- /abr_control/controllers/tests/test_osc.py: -------------------------------------------------------------------------------- 1 | import timeit 2 | 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | import pytest 6 | 7 | from abr_control.arms import jaco2, threejoint, twojoint, ur5 8 | from abr_control.controllers import OSC 9 | from abr_control.utils import transformations 10 | 11 | 12 | @pytest.mark.parametrize( 13 | "arm, ctrlr_dof", 14 | ( 15 | (ur5, [True, True, True, True, True, True]), 16 | (jaco2, [True, True, True, True, True, False]), 17 | ), 18 | ) 19 | def test_velocity_limiting(arm, ctrlr_dof): 20 | # Derivation worked through at studywolf.wordpress.com/2016/11/07/ + 21 | # velocity-limiting-in-operational-space-control/ 22 | robot_config = arm.Config() 23 | 24 | kp = 10 25 | ko = 8 26 | kv = 4 27 | vmax = 1 28 | ctrlr = OSC( 29 | robot_config, kp=kp, ko=ko, kv=kv, ctrlr_dof=ctrlr_dof, vmax=[vmax, vmax] 30 | ) 31 | 32 | answer = np.zeros(6) 33 | # xyz < vmax, abg < vmax 34 | u_task_unscaled = np.array([0.05, 0.05, 0.05, 0.05, 0.05, 0.05]) 35 | answer[:3] = kp * u_task_unscaled[:3] 36 | answer[3:] = ko * u_task_unscaled[3:] 37 | output = ctrlr._velocity_limiting(u_task_unscaled) 38 | assert np.allclose(output, answer, atol=1e-5) 39 | 40 | # xyz > vmax, abg < vmax 41 | u_task_unscaled = np.array([100.0, 100.0, 100.0, 0.05, 0.05, 0.05]) 42 | answer[:3] = kv * np.sqrt(vmax / 3.0) 43 | answer[3:] = ko * u_task_unscaled[3:] 44 | output = ctrlr._velocity_limiting(u_task_unscaled) 45 | assert np.allclose(output, answer, atol=1e-5) 46 | 47 | # xyz < vmax, abg > vmax 48 | u_task_unscaled = np.array([0.05, 0.05, 0.05, 100.0, 100.0, 100.0]) 49 | answer[:3] = kp * u_task_unscaled[:3] 50 | answer[3:] = kv * np.sqrt(vmax / 3.0) 51 | output = ctrlr._velocity_limiting(u_task_unscaled) 52 | assert np.allclose(output, answer, atol=1e-5) 53 | 54 | # xyz > vmax, abg > vmax 55 | u_task_unscaled = np.array([100.0, 100.0, 100.0, 100.0, 100.0, 100.0]) 56 | answer[:3] = kv * np.sqrt(vmax / 3.0) 57 | answer[3:] = kv * np.sqrt(vmax / 3.0) 58 | output = ctrlr._velocity_limiting(u_task_unscaled) 59 | assert np.allclose(output, answer, atol=1e-5) 60 | 61 | 62 | @pytest.mark.parametrize( 63 | "arm, ctrlr_dof", 64 | ( 65 | (ur5, [True, True, True, True, True, True]), 66 | (jaco2, [True, True, True, True, True, False]), 67 | ), 68 | ) 69 | def test_Mx(arm, ctrlr_dof): 70 | robot_config = arm.Config(use_cython=True) 71 | ctrlr = OSC(robot_config, ctrlr_dof=ctrlr_dof) 72 | 73 | for ii in range(100): 74 | q = np.random.random(robot_config.N_JOINTS) * 2 * np.pi 75 | 76 | # test Mx is non-singular case 77 | J = np.eye(robot_config.N_JOINTS) 78 | M = robot_config.M(q=q) 79 | Mx, M_inv = ctrlr._Mx(M=M, J=J, threshold=1e-5) 80 | assert np.allclose(M, Mx, atol=1e-5) 81 | 82 | # test Mx in the singular case 83 | J = np.ones((6, robot_config.N_JOINTS)) 84 | Mx, M_inv = ctrlr._Mx(M=M, J=J) 85 | U2, S2, Vh2 = np.linalg.svd(Mx) 86 | assert np.all(np.abs(S2[1:]) < 1e-10) 87 | 88 | 89 | def calc_distance(Qe, Qd): 90 | dr = Qe[0] * Qd[1:] - Qd[0] * Qe[1:] - np.cross(Qd[1:], Qe[1:]) 91 | return np.linalg.norm(dr, 2) 92 | 93 | 94 | @pytest.mark.parametrize( 95 | "arm, orientation_algorithm", 96 | ( 97 | (threejoint, 0), 98 | (threejoint, 1), 99 | (ur5, 0), 100 | (ur5, 1), 101 | (jaco2, 0), 102 | (jaco2, 1), 103 | ), 104 | ) 105 | def test_calc_orientation_forces(arm, orientation_algorithm): 106 | robot_config = arm.Config(use_cython=False) 107 | ctrlr = OSC(robot_config, orientation_algorithm=orientation_algorithm) 108 | 109 | for ii in range(100): 110 | q = np.random.random(robot_config.N_JOINTS) * 2 * np.pi 111 | quat = robot_config.quaternion("EE", q=q) 112 | 113 | theta = np.pi / 2 114 | axis = np.array([0, 0, 1]) 115 | quat_rot = transformations.unit_vector( 116 | np.hstack([np.cos(theta / 2), np.sin(theta / 2) * axis]) 117 | ) 118 | quat_target = transformations.quaternion_multiply(quat, quat_rot) 119 | target_abg = transformations.euler_from_quaternion(quat_target, axes="rxyz") 120 | 121 | # calculate current position quaternion 122 | R = robot_config.R("EE", q=q) 123 | quat_1 = transformations.unit_vector(transformations.quaternion_from_matrix(R)) 124 | dist1 = calc_distance(quat_1, np.copy(quat_target)) 125 | 126 | # calculate current position quaternion with u_task added 127 | u_task = ctrlr._calc_orientation_forces(target_abg, q=q) 128 | 129 | dq = np.dot( 130 | np.linalg.pinv(robot_config.J("EE", q)), np.hstack([np.zeros(3), u_task]) 131 | ) 132 | q_2 = q - dq * 0.001 # where 0.001 represents the time step 133 | R_2 = robot_config.R("EE", q=q_2) 134 | quat_2 = transformations.unit_vector( 135 | transformations.quaternion_from_matrix(R_2) 136 | ) 137 | 138 | dist2 = calc_distance(quat_2, np.copy(quat_target)) 139 | 140 | assert np.abs(dist2) < np.abs(dist1) 141 | -------------------------------------------------------------------------------- /abr_control/interfaces/__init__.py: -------------------------------------------------------------------------------- 1 | from .coppeliasim import CoppeliaSim 2 | -------------------------------------------------------------------------------- /abr_control/interfaces/coppeliasim_files/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abr/abr_control/0f6e47641d9299422fc24e09ee1445877c235cdc/abr_control/interfaces/coppeliasim_files/__init__.py -------------------------------------------------------------------------------- /abr_control/interfaces/coppeliasim_files/remoteApi.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abr/abr_control/0f6e47641d9299422fc24e09ee1445877c235cdc/abr_control/interfaces/coppeliasim_files/remoteApi.dll -------------------------------------------------------------------------------- /abr_control/interfaces/coppeliasim_files/remoteApi.dylib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abr/abr_control/0f6e47641d9299422fc24e09ee1445877c235cdc/abr_control/interfaces/coppeliasim_files/remoteApi.dylib -------------------------------------------------------------------------------- /abr_control/interfaces/coppeliasim_files/remoteApi.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abr/abr_control/0f6e47641d9299422fc24e09ee1445877c235cdc/abr_control/interfaces/coppeliasim_files/remoteApi.so -------------------------------------------------------------------------------- /abr_control/interfaces/interface.py: -------------------------------------------------------------------------------- 1 | class Interface: 2 | """Base class for interfaces 3 | 4 | The purpose of interfaces is to abstract away the API 5 | and overhead of connection / disconnection etc for each 6 | of the different systems that can be controlled. 7 | 8 | Parameters 9 | ---------- 10 | robot_config : class instance 11 | contains all relevant information about the arm 12 | such as: number of joints, number of links, mass information etc. 13 | """ 14 | 15 | def __init__(self, robot_config): 16 | self.robot_config = robot_config 17 | 18 | def connect(self): 19 | """All initial setup.""" 20 | 21 | raise NotImplementedError 22 | 23 | def disconnect(self): 24 | """Any socket closing etc that must be done to properly shut down""" 25 | 26 | raise NotImplementedError 27 | 28 | def send_forces(self, u): 29 | """Applies the set of torques u to the arm. If interfacing to 30 | a simulation, also moves dynamics forward one time step. 31 | 32 | u : np.array 33 | An array of joint torques [Nm] 34 | """ 35 | 36 | raise NotImplementedError 37 | 38 | def send_target_angles(self, q): 39 | """Moves the arm to the specified joint angles 40 | 41 | q : numpy.array 42 | the target joint angles [radians] 43 | """ 44 | 45 | raise NotImplementedError 46 | 47 | def get_feedback(self): 48 | """Returns a dictionary of the relevant feedback 49 | 50 | Returns a dictionary of relevant feedback to the 51 | controller. At very least this contains q, dq. 52 | """ 53 | 54 | raise NotImplementedError 55 | -------------------------------------------------------------------------------- /abr_control/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abr/abr_control/0f6e47641d9299422fc24e09ee1445877c235cdc/abr_control/utils/__init__.py -------------------------------------------------------------------------------- /abr_control/utils/colors.py: -------------------------------------------------------------------------------- 1 | blue = "\033[94m" 2 | green = "\033[92m" 3 | red = "\033[91m" 4 | yellow = "\u001b[33m" 5 | endc = "\033[0m" 6 | -------------------------------------------------------------------------------- /abr_control/utils/download_meshes.py: -------------------------------------------------------------------------------- 1 | import os 2 | import zipfile 3 | 4 | try: 5 | import requests 6 | except: 7 | print('To download mujoco stl files, you need to "pip install requests"') 8 | 9 | 10 | def check_and_download(name, google_id, files=None, force_download=False): 11 | """ 12 | Checks if the meshes folder exists in the xml directory 13 | If not it will ask the user if they want to download them 14 | to be able to proceed 15 | 16 | Parameters 17 | ---------- 18 | name: string 19 | the file or directory to download 20 | google_id: string 21 | the google id that points to the location of the zip file. 22 | This should be stored in the xml or config file 23 | force_download: boolean, Optional (Default: False) 24 | True to skip checking if the file or folder exists 25 | """ 26 | files_missing = False 27 | 28 | if force_download: 29 | files_missing = True 30 | else: 31 | # check if the provided name is a file or folder 32 | if not os.path.isfile(name) and not os.path.isdir(name): 33 | print("Checking for mesh files in : ", name) 34 | files_missing = True 35 | elif files is not None: 36 | mesh_files = [ 37 | f for f in os.listdir(name) if os.path.isfile(os.path.join(name, f)) 38 | ] 39 | # files_missing = all(elem in sorted(mesh_files) for elem in sorted(files)) 40 | files_missing = set(files).difference(set(mesh_files)) 41 | if files_missing: 42 | print("Checking for mesh files in : ", name) 43 | print("The following files are missing: ", files_missing) 44 | 45 | if files_missing: 46 | yes = ["y", "yes"] 47 | no = ["n", "no"] 48 | answered = False 49 | question = "Download mesh and texture files to run sim? (y/n): " 50 | while not answered: 51 | reply = str(input(question)).lower().strip() 52 | 53 | if reply[0] in yes: 54 | print("Downloading files...") 55 | name = name.split("/") 56 | name = "/".join(s for s in name[:-1]) 57 | download_files(google_id, name + "/tmp") 58 | print(f"Sim files saved to {name}") 59 | answered = True 60 | elif reply[0] in no: 61 | raise Exception("Please download the required files to run the demo") 62 | else: 63 | question = "Please Enter (y/n) " 64 | 65 | 66 | def download_files(google_id, destination): 67 | def _get_confirm_token(response): 68 | for key, value in response.cookies.items(): 69 | if key.startswith("download_warning"): 70 | return value 71 | 72 | return None 73 | 74 | def _save_response_content(response, destination): 75 | CHUNK_SIZE = 32768 76 | 77 | with open(destination, "wb") as f: 78 | for chunk in response.iter_content(CHUNK_SIZE): 79 | if chunk: # filter out keep-alive new chunks 80 | f.write(chunk) 81 | 82 | def _extract_zip_files(zip_file): 83 | zip_file = f"{zip_file}" 84 | with zipfile.ZipFile(zip_file) as zipball: 85 | zipball.extractall(zip_file.split("tmp")[0]) 86 | zipball.close() 87 | os.remove(zip_file) 88 | 89 | URL = "https://docs.google.com/uc?export=download" 90 | 91 | session = requests.Session() 92 | 93 | response = session.get(URL, params={"id": google_id}, stream=True) 94 | token = _get_confirm_token(response) 95 | 96 | if token: 97 | params = {"id": google_id, "confirm": token} 98 | response = session.get(URL, params=params, stream=True) 99 | 100 | _save_response_content(response, destination) 101 | 102 | _extract_zip_files(destination) 103 | -------------------------------------------------------------------------------- /abr_control/utils/os_utils.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | 4 | def makedirs(folder): 5 | """Create the folder if it does not exist 6 | 7 | Checks to see if folder exists, if it does not, then 8 | it creates the folder and all other folders in the path required. 9 | 10 | Parameters 11 | ---------- 12 | folder : string 13 | name of the folder 14 | """ 15 | 16 | if os.path.isdir(folder): 17 | pass 18 | elif os.path.isfile(folder): 19 | raise OSError(f"{folder} exists as a regular file.") 20 | else: 21 | parent, directory = os.path.split(folder) 22 | if parent and not os.path.isdir(parent): 23 | makedirs(parent) 24 | if directory: 25 | os.mkdir(folder) 26 | -------------------------------------------------------------------------------- /abr_control/utils/paths.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | 4 | # Set the path based on the operating system 5 | if sys.platform.startswith("win"): 6 | config_dir = os.path.expanduser(os.path.join("~", ".abr_control")) 7 | cache_dir = os.path.join(config_dir, "cache") 8 | else: 9 | cache_dir = os.path.expanduser(os.path.join("~", ".cache", "abr_control")) 10 | -------------------------------------------------------------------------------- /abr_control/version.py: -------------------------------------------------------------------------------- 1 | name = "abr_control" 2 | version_info = (0, 1, 0) # (major, minor, patch) 3 | dev = False 4 | 5 | v = ".".join(str(v) for v in version_info) 6 | dev_v = ".dev0" if dev else "" 7 | 8 | version = f"{v}{dev_v}" 9 | -------------------------------------------------------------------------------- /examples/CoppeliaSim/force_floating_control.py: -------------------------------------------------------------------------------- 1 | """ 2 | A basic script for connecting to the arm and putting it in floating 3 | mode, which only compensates for gravity. The end-effector position 4 | is recorded and plotted when the script is exited (with ctrl-c). 5 | 6 | In this example, the floating controller is applied in the joint space 7 | """ 8 | import traceback 9 | 10 | import numpy as np 11 | 12 | from abr_control.arms import jaco2 as arm 13 | from abr_control.controllers import Floating 14 | from abr_control.interfaces import CoppeliaSim 15 | 16 | # initialize our robot config 17 | robot_config = arm.Config() 18 | 19 | # instantiate the controller 20 | ctrlr = Floating(robot_config, dynamic=False, task_space=False) 21 | 22 | # create the CoppeliaSim interface and connect up 23 | interface = CoppeliaSim(robot_config, dt=0.005) 24 | interface.connect() 25 | 26 | # set up arrays for tracking end-effector and target position 27 | ee_track = [] 28 | q_track = [] 29 | 30 | 31 | try: 32 | # get the end-effector's initial position 33 | feedback = interface.get_feedback() 34 | start = robot_config.Tx("EE", q=feedback["q"]) 35 | 36 | print("\nSimulation starting...\n") 37 | 38 | while 1: 39 | # get joint angle and velocity feedback 40 | feedback = interface.get_feedback() 41 | 42 | # calculate the control signal 43 | u = ctrlr.generate(q=feedback["q"], dq=feedback["dq"]) 44 | 45 | # send forces into CoppeliaSim 46 | interface.send_forces(u) 47 | 48 | # calculate the position of the hand 49 | hand_xyz = robot_config.Tx("EE", q=feedback["q"]) 50 | # track end effector position 51 | ee_track.append(np.copy(hand_xyz)) 52 | q_track.append(np.copy(feedback["q"])) 53 | 54 | except: 55 | print(traceback.format_exc()) 56 | 57 | finally: 58 | # close the connection to the arm 59 | interface.disconnect() 60 | 61 | print("Simulation terminated...") 62 | 63 | ee_track = np.array(ee_track) 64 | 65 | if ee_track.shape[0] > 0: 66 | # plot distance from target and 3D trajectory 67 | import matplotlib.pyplot as plt 68 | from mpl_toolkits.mplot3d import axes3d # pylint: disable=W0611 69 | 70 | fig = plt.figure(figsize=(8, 12)) 71 | ax1 = fig.add_subplot(211) 72 | ax1.set_title("Joint Angles") 73 | ax1.set_ylabel("Angle (rad)") 74 | ax1.set_xlabel("Time (ms)") 75 | ax1.plot(q_track) 76 | ax1.legend() 77 | 78 | ax2 = fig.add_subplot(212, projection="3d") 79 | ax2.set_title("End-Effector Trajectory") 80 | ax2.plot(ee_track[:, 0], ee_track[:, 1], ee_track[:, 2], label="ee_xyz") 81 | ax2.legend() 82 | plt.show() 83 | -------------------------------------------------------------------------------- /examples/CoppeliaSim/force_osc_abg.py: -------------------------------------------------------------------------------- 1 | """ 2 | Running operational space control using CoppeliaSim. The controller will 3 | move the end-effector to the target object's orientation. 4 | """ 5 | import numpy as np 6 | 7 | from abr_control.arms import ur5 as arm 8 | 9 | # from abr_control.arms import jaco2 as arm 10 | from abr_control.controllers import OSC, Damping 11 | from abr_control.interfaces import CoppeliaSim 12 | from abr_control.utils import transformations 13 | 14 | # initialize our robot config 15 | robot_config = arm.Config() 16 | 17 | # damp the movements of the arm 18 | damping = Damping(robot_config, kv=10) 19 | # create opreational space controller 20 | ctrlr = OSC( 21 | robot_config, 22 | kp=200, # position gain 23 | ko=200, # orientation gain 24 | null_controllers=[damping], 25 | # control (alpha, beta, gamma) out of [x, y, z, alpha, beta, gamma] 26 | ctrlr_dof=[False, False, False, True, True, True], 27 | ) 28 | 29 | # create our interface 30 | interface = CoppeliaSim(robot_config, dt=0.005) 31 | interface.connect() 32 | 33 | # set up lists for tracking data 34 | ee_angles_track = [] 35 | target_angles_track = [] 36 | 37 | 38 | try: 39 | print("\nSimulation starting...\n") 40 | while 1: 41 | # get arm feedback 42 | feedback = interface.get_feedback() 43 | hand_xyz = robot_config.Tx("EE", feedback["q"]) 44 | 45 | target = np.hstack( 46 | [interface.get_xyz("target"), interface.get_orientation("target")] 47 | ) 48 | 49 | rc_matrix = robot_config.R("EE", feedback["q"]) 50 | rc_angles = transformations.euler_from_matrix(rc_matrix, axes="rxyz") 51 | 52 | u = ctrlr.generate( 53 | q=feedback["q"], 54 | dq=feedback["dq"], 55 | target=target, 56 | ) 57 | 58 | # apply the control signal, step the sim forward 59 | interface.send_forces(u) 60 | 61 | # track data 62 | ee_angles_track.append( 63 | transformations.euler_from_matrix( 64 | robot_config.R("EE", feedback["q"]), axes="rxyz" 65 | ) 66 | ) 67 | target_angles_track.append(interface.get_orientation("target")) 68 | 69 | finally: 70 | # stop and reset the simulation 71 | interface.disconnect() 72 | 73 | print("Simulation terminated...") 74 | 75 | ee_angles_track = np.array(ee_angles_track) 76 | target_angles_track = np.array(target_angles_track) 77 | 78 | if ee_angles_track.shape[0] > 0: 79 | # plot distance from target and 3D trajectory 80 | import matplotlib.pyplot as plt 81 | 82 | plt.figure() 83 | plt.plot(ee_angles_track) 84 | plt.gca().set_prop_cycle(None) 85 | plt.plot(target_angles_track, "--") 86 | plt.ylabel("3D orientation (rad)") 87 | plt.xlabel("Time (s)") 88 | plt.tight_layout() 89 | plt.show() 90 | -------------------------------------------------------------------------------- /examples/CoppeliaSim/force_osc_xyz.py: -------------------------------------------------------------------------------- 1 | """ 2 | Move the UR5 CoppeliaSim arm to a target position. 3 | The simulation ends after 1500 time steps, and the 4 | trajectory of the end-effector is plotted in 3D. 5 | """ 6 | import traceback 7 | 8 | import numpy as np 9 | 10 | from abr_control.arms import ur5 as arm 11 | 12 | # from abr_control.arms import jaco2 as arm 13 | # from abr_control.arms import onejoint as arm 14 | from abr_control.controllers import OSC, Damping 15 | from abr_control.interfaces import CoppeliaSim 16 | 17 | # initialize our robot config 18 | robot_config = arm.Config() 19 | 20 | # damp the movements of the arm 21 | damping = Damping(robot_config, kv=10) 22 | # instantiate controller 23 | ctrlr = OSC( 24 | robot_config, 25 | kp=200, 26 | null_controllers=[damping], 27 | vmax=[0.5, 0], # [m/s, rad/s] 28 | # control (x, y, z) out of [x, y, z, alpha, beta, gamma] 29 | ctrlr_dof=[True, True, True, False, False, False], 30 | ) 31 | 32 | # create our CoppeliaSim interface 33 | interface = CoppeliaSim(robot_config, dt=0.005) 34 | interface.connect() 35 | 36 | # set up lists for tracking data 37 | ee_track = [] 38 | target_track = [] 39 | 40 | 41 | try: 42 | # get the end-effector's initial position 43 | feedback = interface.get_feedback() 44 | start = robot_config.Tx("EE", feedback["q"]) 45 | 46 | # make the target offset from that start position 47 | target_xyz = start + np.array([0.2, -0.2, -0.3]) 48 | interface.set_xyz(name="target", xyz=target_xyz) 49 | 50 | count = 0.0 51 | print("\nSimulation starting...\n") 52 | while count < 1500: 53 | # get joint angle and velocity feedback 54 | feedback = interface.get_feedback() 55 | 56 | target = np.hstack( 57 | [interface.get_xyz("target"), interface.get_orientation("target")] 58 | ) 59 | 60 | name = "joint1" 61 | vrep_angles = interface.get_orientation(f"UR5_{name}") 62 | 63 | # calculate the control signal 64 | u = ctrlr.generate( 65 | q=feedback["q"], 66 | dq=feedback["dq"], 67 | target=target, 68 | ) 69 | 70 | # send forces into CoppeliaSim, step the sim forward 71 | interface.send_forces(u) 72 | 73 | # calculate end-effector position 74 | ee_xyz = robot_config.Tx("EE", q=feedback["q"]) 75 | # track data 76 | ee_track.append(np.copy(ee_xyz)) 77 | target_track.append(np.copy(target[:3])) 78 | 79 | count += 1 80 | 81 | except: 82 | print(traceback.format_exc()) 83 | 84 | finally: 85 | # stop and reset the CoppeliaSim simulation 86 | interface.disconnect() 87 | 88 | print("Simulation terminated...") 89 | 90 | ee_track = np.array(ee_track) 91 | target_track = np.array(target_track) 92 | 93 | if ee_track.shape[0] > 0: 94 | # plot distance from target and 3D trajectory 95 | import matplotlib.pyplot as plt 96 | from mpl_toolkits.mplot3d import axes3d # pylint: disable=W0611 97 | 98 | fig = plt.figure(figsize=(8, 12)) 99 | ax1 = fig.add_subplot(211) 100 | ax1.set_ylabel("Distance (m)") 101 | ax1.set_xlabel("Time (ms)") 102 | ax1.set_title("Distance to target") 103 | ax1.plot( 104 | np.sqrt(np.sum((np.array(target_track) - np.array(ee_track)) ** 2, axis=1)) 105 | ) 106 | 107 | ax2 = fig.add_subplot(212, projection="3d") 108 | ax2.set_title("End-Effector Trajectory") 109 | ax2.plot(ee_track[:, 0], ee_track[:, 1], ee_track[:, 2], label="ee_xyz") 110 | ax2.scatter( 111 | target_track[0, 0], 112 | target_track[0, 1], 113 | target_track[0, 2], 114 | label="target", 115 | c="r", 116 | ) 117 | ax2.legend() 118 | plt.show() 119 | -------------------------------------------------------------------------------- /examples/CoppeliaSim/force_osc_xyz_avoid_obstacle.py: -------------------------------------------------------------------------------- 1 | """ 2 | Move the UR5 CoppeliaSim arm to a target position while avoiding an obstacle. 3 | The simulation ends after 1500 time steps, and the trajectory 4 | of the end-effector is plotted in 3D. 5 | """ 6 | import numpy as np 7 | 8 | from abr_control.arms import ur5 as arm 9 | 10 | # from abr_control.arms import jaco2 as arm 11 | from abr_control.controllers import OSC, AvoidObstacles, Damping 12 | from abr_control.interfaces import CoppeliaSim 13 | 14 | # initialize our robot config 15 | robot_config = arm.Config() 16 | 17 | avoid = AvoidObstacles(robot_config) 18 | # damp the movements of the arm 19 | damping = Damping(robot_config, kv=10) 20 | # instantiate the REACH controller with obstacle avoidance 21 | ctrlr = OSC( 22 | robot_config, 23 | kp=200, 24 | null_controllers=[avoid, damping], 25 | vmax=[0.5, 0], # [m/s, rad/s] 26 | # control (x, y, z) out of [x, y, z, alpha, beta, gamma] 27 | ctrlr_dof=[True, True, True, False, False, False], 28 | ) 29 | 30 | # create our CoppeliaSim interface 31 | interface = CoppeliaSim(robot_config, dt=0.005) 32 | interface.connect() 33 | 34 | # set up lists for tracking data 35 | ee_track = [] 36 | target_track = [] 37 | obstacle_track = [] 38 | 39 | moving_obstacle = True 40 | obstacle_xyz = np.array([0.09596, -0.2661, 0.64204]) 41 | 42 | 43 | try: 44 | # get visual position of end point of object 45 | feedback = interface.get_feedback() 46 | start = robot_config.Tx("EE", q=feedback["q"]) 47 | 48 | # make the target offset from that start position 49 | target_xyz = start + np.array([0.2, -0.2, 0.0]) 50 | interface.set_xyz(name="target", xyz=target_xyz) 51 | interface.set_xyz(name="obstacle", xyz=obstacle_xyz) 52 | 53 | count = 0.0 54 | obs_count = 0.0 55 | print("\nSimulation starting...\n") 56 | while count < 1500: 57 | # get joint angle and velocity feedback 58 | feedback = interface.get_feedback() 59 | 60 | target = np.hstack( 61 | [interface.get_xyz("target"), interface.get_orientation("target")] 62 | ) 63 | 64 | # calculate the control signal 65 | u = ctrlr.generate( 66 | q=feedback["q"], 67 | dq=feedback["dq"], 68 | target=target, 69 | ) 70 | 71 | # get obstacle position from CoppeliaSim 72 | obs_x, obs_y, obs_z = interface.get_xyz("obstacle") # pylint: disable=W0632 73 | # update avoidance system about obstacle position 74 | avoid.set_obstacles([[obs_x, obs_y, obs_z, 0.05]]) 75 | if moving_obstacle is True: 76 | obs_x = 0.125 + 0.25 * np.sin(obs_count) 77 | obs_count += 0.05 78 | interface.set_xyz(name="obstacle", xyz=[obs_x, obs_y, obs_z]) 79 | 80 | # send forces into CoppeliaSim, step the sim forward 81 | interface.send_forces(u) 82 | 83 | # calculate end-effector position 84 | ee_xyz = robot_config.Tx("EE", q=feedback["q"]) 85 | # track data 86 | ee_track.append(np.copy(ee_xyz)) 87 | target_track.append(np.copy(target[:3])) 88 | obstacle_track.append(np.copy([obs_x, obs_y, obs_z])) 89 | 90 | count += 1 91 | 92 | finally: 93 | # stop and reset the CoppeliaSim simulation 94 | interface.disconnect() 95 | 96 | print("Simulation terminated...") 97 | 98 | ee_track = np.array(ee_track) 99 | target_track = np.array(target_track) 100 | obstacle_track = np.array(obstacle_track) 101 | 102 | if ee_track.shape[0] > 0: 103 | # plot distance from target and 3D trajectory 104 | import matplotlib.pyplot as plt 105 | from mpl_toolkits.mplot3d import axes3d # pylint: disable=W0611 106 | 107 | fig = plt.figure(figsize=(8, 12)) 108 | ax1 = fig.add_subplot(211) 109 | ax1.set_ylabel("Distance (m)") 110 | ax1.set_xlabel("Time (ms)") 111 | ax1.set_title("Distance to target") 112 | ax1.plot( 113 | np.sqrt(np.sum((np.array(target_track) - np.array(ee_track)) ** 2, axis=1)) 114 | ) 115 | 116 | ax2 = fig.add_subplot(212, projection="3d") 117 | ax2.set_title("End-Effector Trajectory") 118 | ax2.plot(ee_track[:, 0], ee_track[:, 1], ee_track[:, 2], label="ee_xyz") 119 | ax2.scatter( 120 | target_track[0, 0], 121 | target_track[0, 1], 122 | target_track[0, 2], 123 | label="target", 124 | c="g", 125 | ) 126 | ax2.plot( 127 | obstacle_track[:, 0], 128 | obstacle_track[:, 1], 129 | target_track[:, 2], 130 | label="obstacle", 131 | c="r", 132 | ) 133 | ax2.legend() 134 | plt.show() 135 | -------------------------------------------------------------------------------- /examples/CoppeliaSim/force_osc_xyz_dynamics_adaptation.py: -------------------------------------------------------------------------------- 1 | """ 2 | Move the UR5 CoppeliaSim arm to a target position. 3 | The simulation ends after 1.5 simulated seconds, and the 4 | trajectory of the end-effector is plotted in 3D. 5 | """ 6 | import traceback 7 | 8 | import numpy as np 9 | 10 | from abr_control.arms import jaco2 as arm 11 | from abr_control.controllers import OSC, Damping, signals 12 | from abr_control.interfaces import CoppeliaSim 13 | 14 | # initialize our robot config for the jaco2 15 | robot_config = arm.Config() 16 | 17 | # damp the movements of the arm 18 | damping = Damping(robot_config, kv=10) 19 | # instantiate controller 20 | ctrlr = OSC( 21 | robot_config, 22 | kp=200, 23 | null_controllers=[damping], 24 | vmax=[0.5, 0], # [m/s, rad/s] 25 | # control (x, y, z) out of [x, y, z, alpha, beta, gamma] 26 | ctrlr_dof=[True, True, True, False, False, False], 27 | ) 28 | 29 | # create our adaptive controller 30 | adapt = signals.DynamicsAdaptation( 31 | n_neurons=1000, 32 | n_ensembles=5, 33 | n_input=2, # we apply adaptation on the most heavily stressed joints 34 | n_output=2, 35 | pes_learning_rate=5e-5, 36 | means=[3.14, 3.14], 37 | variances=[1.57, 1.57], 38 | ) 39 | 40 | # create our CoppeliaSim interface 41 | interface = CoppeliaSim(robot_config, dt=0.005) 42 | interface.connect() 43 | interface.send_target_angles(q=robot_config.START_ANGLES) 44 | 45 | # set up lists for tracking data 46 | ee_track = [] 47 | target_track = [] 48 | 49 | # get Jacobians to each link for calculating perturbation 50 | J_links = [ 51 | robot_config._calc_J(f"link{ii}", x=[0, 0, 0]) for ii in range(robot_config.N_LINKS) 52 | ] 53 | 54 | 55 | try: 56 | # get the end-effector's initial position 57 | feedback = interface.get_feedback() 58 | start = robot_config.Tx("EE", feedback["q"]) 59 | 60 | # make the target offset from that start position 61 | target_xyz = start + np.array([0.2, -0.2, 0.0]) 62 | interface.set_xyz(name="target", xyz=target_xyz) 63 | 64 | count = 0.0 65 | while 1: 66 | # get joint angle and velocity feedback 67 | feedback = interface.get_feedback() 68 | 69 | target = np.hstack( 70 | [interface.get_xyz("target"), interface.get_orientation("target")] 71 | ) 72 | 73 | # calculate the control signal 74 | u = ctrlr.generate( 75 | q=feedback["q"], 76 | dq=feedback["dq"], 77 | target=target, 78 | ) 79 | 80 | u_adapt = np.zeros(robot_config.N_JOINTS) 81 | u_adapt[1:3] = adapt.generate( 82 | input_signal=np.array([feedback["q"][1], feedback["q"][2]]), 83 | training_signal=np.array( 84 | [ctrlr.training_signal[1], ctrlr.training_signal[2]] 85 | ), 86 | ) 87 | u += u_adapt 88 | 89 | # add an additional force for the controller to adapt to 90 | extra_gravity = robot_config.g(feedback["q"]) * 2 91 | u += extra_gravity 92 | 93 | # send forces into CoppeliaSim, step the sim forward 94 | interface.send_forces(u) 95 | 96 | # calculate end-effector position 97 | ee_xyz = robot_config.Tx("EE", q=feedback["q"]) 98 | # track data 99 | ee_track.append(np.copy(ee_xyz)) 100 | target_track.append(np.copy(target[:3])) 101 | 102 | count += 1 103 | 104 | ee_xyz = robot_config.Tx("EE", q=feedback["q"]) 105 | interface.set_xyz(name="hand", xyz=ee_xyz) 106 | 107 | except: 108 | print(traceback.format_exc()) 109 | 110 | finally: 111 | # stop and reset the CoppeliaSim simulation 112 | interface.disconnect() 113 | 114 | ee_track = np.array(ee_track) 115 | target_track = np.array(target_track) 116 | 117 | if ee_track.shape[0] > 0: 118 | # plot distance from target and 3D trajectory 119 | import matplotlib.pyplot as plt 120 | from mpl_toolkits.mplot3d import axes3d # pylint: disable=W0611 121 | 122 | fig = plt.figure(figsize=(8, 12)) 123 | ax1 = fig.add_subplot(211) 124 | ax1.set_ylabel("Distance (m)") 125 | ax1.set_xlabel("Time (ms)") 126 | ax1.set_title("Distance to target") 127 | ax1.plot( 128 | np.sqrt(np.sum((np.array(target_track) - np.array(ee_track)) ** 2, axis=1)) 129 | ) 130 | 131 | ax2 = fig.add_subplot(212, projection="3d") 132 | ax2.set_title("End-Effector Trajectory") 133 | ax2.plot(ee_track[:, 0], ee_track[:, 1], ee_track[:, 2], label="ee_xyz") 134 | ax2.scatter(target_xyz[0], target_xyz[1], target_xyz[2], label="target", c="r") 135 | ax2.legend() 136 | plt.show() 137 | -------------------------------------------------------------------------------- /examples/CoppeliaSim/force_osc_xyzabg.py: -------------------------------------------------------------------------------- 1 | """ 2 | Running operational space control using CoppeliaSim. The controller will 3 | move the end-effector to the target object's position and orientation. 4 | """ 5 | import numpy as np 6 | 7 | from abr_control.arms import ur5 as arm 8 | from abr_control.controllers import OSC, Damping 9 | from abr_control.interfaces import CoppeliaSim 10 | from abr_control.utils import transformations 11 | 12 | # initialize our robot config 13 | robot_config = arm.Config() 14 | 15 | # damp the movements of the arm 16 | damping = Damping(robot_config, kv=10) 17 | # create opreational space controller 18 | ctrlr_dof = np.array([True, False, False, True, True, True]) 19 | ctrlr = OSC( 20 | robot_config, 21 | kp=200, 22 | null_controllers=[damping], 23 | vmax=[10, 10], # [m/s, rad/s] 24 | # control (x, alpha, beta, gamma) out of [x, y, z, alpha, beta, gamma] 25 | ctrlr_dof=ctrlr_dof, 26 | ) 27 | 28 | # create our interface 29 | interface = CoppeliaSim(robot_config, dt=0.005) 30 | interface.connect() 31 | 32 | # set up lists for tracking data 33 | ee_track = [] 34 | ee_angles_track = [] 35 | target_track = [] 36 | target_angles_track = [] 37 | 38 | 39 | try: 40 | count = 0 41 | print("\nSimulation starting...\n") 42 | while 1: 43 | # get arm feedback 44 | feedback = interface.get_feedback() 45 | hand_xyz = robot_config.Tx("EE", feedback["q"]) 46 | 47 | target = np.hstack( 48 | [interface.get_xyz("target"), interface.get_orientation("target")] 49 | ) 50 | 51 | u = ctrlr.generate( 52 | q=feedback["q"], 53 | dq=feedback["dq"], 54 | target=target, 55 | ) 56 | 57 | # apply the control signal, step the sim forward 58 | interface.send_forces(u) 59 | 60 | # track data 61 | ee_track.append(np.copy(hand_xyz)) 62 | ee_angles_track.append( 63 | transformations.euler_from_matrix( 64 | robot_config.R("EE", feedback["q"]), axes="rxyz" 65 | ) 66 | ) 67 | tmp_target = np.copy(hand_xyz[:3]) 68 | tmp_target[ctrlr_dof[:3]] = np.copy(target[:3][ctrlr_dof[:3]]) 69 | target_track.append(tmp_target) # np.copy(target[:3])) 70 | target_angles_track.append(interface.get_orientation("target")) 71 | count += 1 72 | 73 | finally: 74 | # stop and reset the simulation 75 | interface.disconnect() 76 | 77 | print("Simulation terminated...") 78 | 79 | ee_track = np.array(ee_track) 80 | ee_angles_track = np.array(ee_angles_track) 81 | target_track = np.array(target_track) 82 | target_angles_track = np.array(target_angles_track) 83 | 84 | if ee_track.shape[0] > 0: 85 | # plot distance from target and 3D trajectory 86 | import matplotlib.pyplot as plt 87 | from mpl_toolkits.mplot3d import axes3d # pylint: disable=W0611 88 | 89 | fig = plt.figure(figsize=(8, 12)) 90 | ax1 = fig.add_subplot(311) 91 | ax1.set_ylabel("3D position (m)") 92 | ax1.plot(ee_track) 93 | ax1.plot(target_track, "--") 94 | 95 | ax2 = fig.add_subplot(312) 96 | ax2.plot(ee_angles_track) 97 | ax2.plot(target_angles_track, "--") 98 | ax2.set_ylabel("3D orientation (rad)") 99 | ax2.set_xlabel("Time (s)") 100 | 101 | ax3 = fig.add_subplot(313, projection="3d") 102 | ax3.set_title("End-Effector Trajectory") 103 | ax3.plot(ee_track[:, 0], ee_track[:, 1], ee_track[:, 2], label="ee_xyz") 104 | ax3.scatter( 105 | ee_track[-1, 0], ee_track[-1, 1], ee_track[-1, 2], label="final_loc", c="r" 106 | ) 107 | ax3.scatter( 108 | target_track[-1, 0], 109 | target_track[-1, 1], 110 | target_track[-1, 2], 111 | label="target", 112 | c="g", 113 | ) 114 | ax3.legend() 115 | plt.show() 116 | -------------------------------------------------------------------------------- /examples/CoppeliaSim/force_sliding_control_xyz.py: -------------------------------------------------------------------------------- 1 | """ 2 | Move the UR5 CoppeliaSim arm to a target position. 3 | The simulation ends after 1500 time steps, and the 4 | trajectory of the end-effector is plotted in 3D. 5 | """ 6 | import traceback 7 | 8 | import numpy as np 9 | 10 | # from abr_control.arms import ur5 as arm 11 | from abr_control.arms import jaco2 as arm 12 | 13 | # from abr_control.arms import onejoint as arm 14 | from abr_control.controllers import Sliding 15 | from abr_control.interfaces import CoppeliaSim 16 | 17 | # initialize our robot config 18 | robot_config = arm.Config() 19 | 20 | # instantiate controller 21 | # NOTE: These values are non-optimal 22 | ctrlr = Sliding(robot_config, kd=10.0, lamb=30.00) 23 | 24 | # create our CoppeliaSim interface 25 | interface = CoppeliaSim(robot_config, dt=0.001) 26 | interface.connect() 27 | 28 | # set up lists for tracking data 29 | ee_track = [] 30 | target_track = [] 31 | 32 | 33 | try: 34 | # get the end-effector's initial position 35 | feedback = interface.get_feedback() 36 | start = robot_config.Tx("EE", feedback["q"]) 37 | 38 | # make the target offset from that start position 39 | target_xyz = start + np.array([0.2, -0.2, 0.0]) 40 | interface.set_xyz(name="target", xyz=target_xyz) 41 | 42 | count = 0.0 43 | print("\nSimulation starting...\n") 44 | while count < 1500: 45 | # get joint angle and velocity feedback 46 | feedback = interface.get_feedback() 47 | 48 | # calculate the control signal 49 | u = ctrlr.generate(q=feedback["q"], dq=feedback["dq"], target=target_xyz) 50 | 51 | # send forces into CoppeliaSim, step the sim forward 52 | interface.send_forces(u) 53 | 54 | # calculate end-effector position 55 | ee_xyz = robot_config.Tx("EE", q=feedback["q"]) 56 | # track data 57 | ee_track.append(np.copy(ee_xyz)) 58 | target_track.append(np.copy(target_xyz)) 59 | 60 | count += 1 61 | 62 | except: 63 | print(traceback.format_exc()) 64 | 65 | finally: 66 | # stop and reset the CoppeliaSim simulation 67 | interface.disconnect() 68 | 69 | print("Simulation terminated...") 70 | 71 | ee_track = np.array(ee_track) 72 | target_track = np.array(target_track) 73 | 74 | if ee_track.shape[0] > 0: 75 | # plot distance from target and 3D trajectory 76 | import matplotlib.pyplot as plt 77 | from mpl_toolkits.mplot3d import axes3d # pylint: disable=W0611 78 | 79 | fig = plt.figure(figsize=(8, 12)) 80 | ax1 = fig.add_subplot(211) 81 | ax1.set_ylabel("Distance (m)") 82 | ax1.set_xlabel("Time (ms)") 83 | ax1.set_title("Distance to target") 84 | ax1.plot( 85 | np.sqrt(np.sum((np.array(target_track) - np.array(ee_track)) ** 2, axis=1)) 86 | ) 87 | 88 | ax2 = fig.add_subplot(212, projection="3d") 89 | ax2.set_title("End-Effector Trajectory") 90 | ax2.plot(ee_track[:, 0], ee_track[:, 1], ee_track[:, 2], label="ee_xyz") 91 | ax2.scatter( 92 | target_track[0, 0], 93 | target_track[0, 1], 94 | target_track[0, 2], 95 | label="target", 96 | c="r", 97 | ) 98 | ax2.legend() 99 | plt.show() 100 | -------------------------------------------------------------------------------- /examples/CoppeliaSim/position_joint_control.py: -------------------------------------------------------------------------------- 1 | """ 2 | A basic script for connecting and moving the arm to a target 3 | configuration in joint space, offset from its starting position. 4 | The simulation simulates 1500 time steps and then plots the results. 5 | """ 6 | import traceback 7 | 8 | import numpy as np 9 | 10 | # from abr_control.arms import ur5 as arm 11 | from abr_control.arms import jaco2 as arm 12 | 13 | # from abr_control.arms import onejoint as arm 14 | from abr_control.controllers import Joint 15 | from abr_control.interfaces import CoppeliaSim 16 | 17 | # initialize our robot config 18 | robot_config = arm.Config() 19 | 20 | # instantiate the REACH controller for the jaco2 robot 21 | ctrlr = Joint(robot_config, kp=50) 22 | 23 | # create interface and connect 24 | interface = CoppeliaSim(robot_config=robot_config, dt=0.005) 25 | interface.connect() 26 | 27 | # make the target an offset of the current configuration 28 | feedback = interface.get_feedback() 29 | target = feedback["q"] + np.ones(robot_config.N_JOINTS) * 0.3 30 | 31 | # For CoppeliaSim files that have a shadow arm to show the target configuration 32 | # get joint handles for shadow 33 | names = [f"joint{ii}_shadow" for ii in range(robot_config.N_JOINTS)] 34 | joint_handles = [] 35 | for name in names: 36 | interface.get_xyz(name) # this loads in the joint handle 37 | joint_handles.append(interface.misc_handles[name]) 38 | # move shadow to target position 39 | interface.send_target_angles(target, joint_handles) 40 | 41 | # set up arrays for tracking end-effector and target position 42 | q_track = [] 43 | 44 | 45 | try: 46 | count = 0 47 | print("\nSimulation starting...\n") 48 | while count < 1500: 49 | # get joint angle and velocity feedback 50 | feedback = interface.get_feedback() 51 | 52 | # calculate the control signal 53 | u = ctrlr.generate( 54 | q=feedback["q"], 55 | dq=feedback["dq"], 56 | target=target, 57 | ) 58 | 59 | # send forces into CoppeliaSim, step the sim forward 60 | interface.send_forces(u) 61 | 62 | # track joint angles 63 | q_track.append(np.copy(feedback["q"])) 64 | count += 1 65 | 66 | except: 67 | print(traceback.format_exc()) 68 | 69 | finally: 70 | # close the connection to the arm 71 | interface.disconnect() 72 | 73 | print("Simulation terminated...") 74 | 75 | q_track = np.array(q_track) 76 | if q_track.shape[0] > 0: 77 | import matplotlib.pyplot as plt 78 | 79 | plt.plot((q_track + np.pi) % (np.pi * 2) - np.pi) 80 | plt.gca().set_prop_cycle(None) 81 | plt.plot( 82 | np.ones(q_track.shape) * ((target + np.pi) % (np.pi * 2) - np.pi), "--" 83 | ) 84 | plt.legend(range(robot_config.N_LINKS)) 85 | plt.tight_layout() 86 | plt.show() 87 | -------------------------------------------------------------------------------- /examples/Mujoco/force_floating_control.py: -------------------------------------------------------------------------------- 1 | """ 2 | A basic script for connecting to the arm and putting it in floating 3 | mode, which only compensates for gravity. The end-effector position 4 | is recorded and plotted when the script is exited (with ctrl-c). 5 | 6 | In this example, the floating controller is applied in the joint space 7 | """ 8 | import sys 9 | import traceback 10 | 11 | import glfw 12 | import numpy as np 13 | 14 | from abr_control.arms.mujoco_config import MujocoConfig as arm 15 | from abr_control.controllers import Floating 16 | from abr_control.interfaces.mujoco import Mujoco 17 | 18 | if len(sys.argv) > 1: 19 | arm_model = sys.argv[1] 20 | else: 21 | arm_model = "jaco2" 22 | # initialize our robot config 23 | robot_config = arm(arm_model) 24 | 25 | # create the Mujoco interface and connect up 26 | interface = Mujoco(robot_config, dt=0.001) 27 | interface.connect(joint_names=[f"joint{ii}" for ii in range(len(robot_config.START_ANGLES))]) 28 | interface.send_target_angles(robot_config.START_ANGLES) 29 | 30 | # instantiate the controller 31 | ctrlr = Floating(robot_config, task_space=False, dynamic=True) 32 | 33 | # set up arrays for tracking end-effector and target position 34 | ee_track = [] 35 | q_track = [] 36 | 37 | 38 | try: 39 | # get the end-effector's initial position 40 | feedback = interface.get_feedback() 41 | start = robot_config.Tx("EE", q=feedback["q"]) 42 | 43 | print("\nSimulation starting...\n") 44 | 45 | while 1: 46 | if glfw.window_should_close(interface.viewer.window): 47 | break 48 | # get joint angle and velocity feedback 49 | feedback = interface.get_feedback() 50 | 51 | # calculate the control signal 52 | u = ctrlr.generate(q=feedback["q"], dq=feedback["dq"]) 53 | 54 | # send forces into Mujoco 55 | interface.send_forces(u) 56 | 57 | # calculate the position of the hand 58 | hand_xyz = robot_config.Tx("EE", q=feedback["q"]) 59 | # track end effector position 60 | ee_track.append(np.copy(hand_xyz)) 61 | q_track.append(np.copy(feedback["q"])) 62 | 63 | except: 64 | print(traceback.format_exc()) 65 | 66 | finally: 67 | # close the connection to the arm 68 | interface.disconnect() 69 | 70 | print("Simulation terminated...") 71 | 72 | ee_track = np.array(ee_track) 73 | 74 | if ee_track.shape[0] > 0: 75 | # plot distance from target and 3D trajectory 76 | import matplotlib.pyplot as plt 77 | from mpl_toolkits.mplot3d import axes3d # pylint: disable=W0611 78 | 79 | fig = plt.figure(figsize=(8, 12)) 80 | ax1 = fig.add_subplot(211) 81 | ax1.set_title("Joint Angles") 82 | ax1.set_ylabel("Angle (rad)") 83 | ax1.set_xlabel("Time (ms)") 84 | ax1.plot(q_track) 85 | ax1.legend() 86 | 87 | ax2 = fig.add_subplot(212, projection="3d") 88 | ax2.set_title("End-Effector Trajectory") 89 | ax2.plot(ee_track[:, 0], ee_track[:, 1], ee_track[:, 2], label="ee_xyz") 90 | ax2.legend() 91 | plt.show() 92 | -------------------------------------------------------------------------------- /examples/Mujoco/force_joint_control.py: -------------------------------------------------------------------------------- 1 | """ 2 | A basic script for connecting and moving the arm to a target 3 | configuration in joint space, offset from its starting position. 4 | The simulation simulates 2500 time steps and then plots the results. 5 | 6 | NOTE: The number of joints controlled is specified by the 7 | n_dof_to_control variable below 8 | """ 9 | import sys 10 | import traceback 11 | 12 | import glfw 13 | import numpy as np 14 | 15 | from abr_control.arms.mujoco_config import MujocoConfig as arm 16 | from abr_control.controllers import Joint 17 | from abr_control.interfaces.mujoco import Mujoco 18 | 19 | if len(sys.argv) > 1: 20 | arm_model = sys.argv[1] 21 | else: 22 | arm_model = "jaco2" 23 | # initialize our robot config for the jaco2 24 | robot_config = arm(arm_model) 25 | 26 | # create interface and connect 27 | interface = Mujoco(robot_config=robot_config, dt=0.001) 28 | 29 | n_dof_to_control = 3 30 | interface.connect(joint_names=[f"joint{ii}" for ii in range(n_dof_to_control)]) 31 | interface.send_target_angles(robot_config.START_ANGLES[:n_dof_to_control]) 32 | 33 | # instantiate the REACH controller for the jaco2 robot 34 | ctrlr = Joint(robot_config, kp=20, kv=10) 35 | 36 | # make the target an offset of the current configuration 37 | feedback = interface.get_feedback() 38 | target = feedback["q"] + np.random.random(robot_config.N_JOINTS) * 1 - 0.5 39 | 40 | # set up arrays for tracking end-effector and target position 41 | q_track = [] 42 | 43 | 44 | try: 45 | count = 0 46 | print("\nSimulation starting...\n") 47 | while count < 2500: 48 | if glfw.window_should_close(interface.viewer.window): 49 | break 50 | # get joint angle and velocity feedback 51 | feedback = interface.get_feedback() 52 | 53 | # calculate the control signal 54 | u = ctrlr.generate( 55 | q=feedback["q"], 56 | dq=feedback["dq"], 57 | target=target, 58 | ) 59 | 60 | # send forces into Mujoco, step the sim forward 61 | interface.send_forces(u) 62 | 63 | # track joint angles 64 | q_track.append(np.copy(feedback["q"])) 65 | count += 1 66 | 67 | except: 68 | print(traceback.format_exc()) 69 | 70 | finally: 71 | # close the connection to the arm 72 | interface.disconnect() 73 | 74 | print("Simulation terminated...") 75 | 76 | q_track = np.array(q_track) 77 | if q_track.shape[0] > 0: 78 | import matplotlib.pyplot as plt 79 | 80 | plt.plot((q_track + np.pi) % (np.pi * 2) - np.pi) 81 | plt.gca().set_prop_cycle(None) 82 | plt.plot( 83 | np.ones(q_track.shape) * ((target + np.pi) % (np.pi * 2) - np.pi), "--" 84 | ) 85 | plt.legend(range(robot_config.N_JOINTS)) 86 | plt.tight_layout() 87 | plt.show() 88 | -------------------------------------------------------------------------------- /examples/Mujoco/force_joint_control_balljoint.py: -------------------------------------------------------------------------------- 1 | """ 2 | Move the jao2 Mujoco arm to a target position. 3 | The simulation ends after 1500 time steps, and the 4 | trajectory of the end-effector is plotted in 3D. 5 | """ 6 | import traceback 7 | 8 | import glfw 9 | import numpy as np 10 | 11 | from abr_control.arms.mujoco_config import MujocoConfig as arm 12 | from abr_control.controllers import Joint 13 | from abr_control.interfaces.mujoco import Mujoco 14 | from abr_control.utils import transformations 15 | from abr_control.utils.transformations import quaternion_conjugate, quaternion_multiply 16 | 17 | # initialize our robot config for the jaco2 18 | robot_config = arm("mujoco_balljoint.xml", folder=".", use_sim_state=False) 19 | 20 | # create our Mujoco interface 21 | interface = Mujoco(robot_config, dt=0.001) 22 | interface.connect() 23 | 24 | # instantiate controller 25 | kp = 100 26 | kv = np.sqrt(kp) 27 | ctrlr = Joint( 28 | robot_config, 29 | kp=kp, 30 | kv=kv, 31 | quaternions=[True], 32 | ) 33 | 34 | # set up lists for tracking data 35 | q_track = [] 36 | target_track = [] 37 | error_track = [] 38 | 39 | green = [0, 0.9, 0, 0.5] 40 | red = [0.9, 0, 0, 0.5] 41 | threshold = 0.002 # threshold distance for being within target before moving on 42 | 43 | # get the end-effector's initial position 44 | np.random.seed(0) 45 | target_quaternions = [ 46 | transformations.unit_vector(transformations.random_quaternion()) for ii in range(4) 47 | ] 48 | 49 | target_index = 0 50 | target = target_quaternions[target_index] 51 | print(target) 52 | target_xyz = robot_config.Tx("EE", q=target) 53 | interface.set_mocap_xyz(name="target", xyz=target_xyz) 54 | 55 | try: 56 | # get the end-effector's initial position 57 | feedback = interface.get_feedback() 58 | start = robot_config.Tx("EE", feedback["q"]) 59 | 60 | count = 0.0 61 | print("\nSimulation starting...\n") 62 | while 1: 63 | if glfw.window_should_close(interface.viewer.window): 64 | break 65 | # get joint angle and velocity feedback 66 | feedback = interface.get_feedback() 67 | 68 | # calculate the control signal 69 | u = ctrlr.generate( 70 | q=feedback["q"], 71 | dq=feedback["dq"], 72 | target=target, 73 | ) 74 | 75 | # send forces into Mujoco, step the sim forward 76 | interface.send_forces(u) 77 | 78 | # track data 79 | q_track.append(np.copy(feedback["q"])) 80 | target_track.append(np.copy(target)) 81 | 82 | # calculate the distance between quaternions 83 | error = quaternion_multiply( 84 | target, 85 | quaternion_conjugate(feedback["q"]), 86 | ) 87 | error = 2 * np.arctan2(np.linalg.norm(error[1:]) * -np.sign(error[0]), error[0]) 88 | # quaternion distance for same angles can be 0 or 2*pi, so use a sine 89 | # wave here so 0 and 2*np.pi = 0 90 | error = np.sin(error / 2) 91 | error_track.append(np.copy(error)) 92 | 93 | if abs(error) < threshold: 94 | interface.model.geom("target").rgba = green 95 | count += 1 96 | else: 97 | count = 0 98 | interface.model.geom("target").rgba = red 99 | 100 | if count >= 1000: 101 | target_index += 1 102 | target = target_quaternions[target_index % len(target_quaternions)] 103 | target_xyz = robot_config.Tx("EE", q=target) 104 | interface.set_mocap_xyz(name="target", xyz=target_xyz) 105 | count = 0 106 | 107 | except: 108 | print(traceback.format_exc()) 109 | 110 | finally: 111 | # stop and reset the Mujoco simulation 112 | interface.disconnect() 113 | 114 | print("Simulation terminated...") 115 | 116 | q_track = np.array(q_track) 117 | target_track = np.array(target_track) 118 | 119 | if q_track.shape[0] > 0: 120 | # plot distance from target and 3D trajectory 121 | import matplotlib.pyplot as plt 122 | from mpl_toolkits.mplot3d import axes3d # pylint: disable=W0611 123 | 124 | plt.figure(figsize=(8, 6)) 125 | plt.ylabel("Distance (m)") 126 | plt.xlabel("Time (ms)") 127 | plt.title("Distance to target") 128 | plt.plot(np.array(error_track)) 129 | plt.show() 130 | -------------------------------------------------------------------------------- /examples/Mujoco/force_joint_control_two_balljoints.py: -------------------------------------------------------------------------------- 1 | """ 2 | Move the jao2 Mujoco arm to a target position. 3 | The simulation ends after 1500 time steps, and the 4 | trajectory of the end-effector is plotted in 3D. 5 | """ 6 | import traceback 7 | 8 | import glfw 9 | import numpy as np 10 | 11 | from abr_control.arms.mujoco_config import MujocoConfig as arm 12 | from abr_control.controllers import Joint 13 | from abr_control.interfaces.mujoco import Mujoco 14 | from abr_control.utils import transformations 15 | from abr_control.utils.transformations import quaternion_conjugate, quaternion_multiply 16 | 17 | # initialize our robot config for the jaco2 18 | robot_config = arm("mujoco_two_balljoints.xml", folder=".", use_sim_state=False) 19 | 20 | # create our Mujoco interface 21 | interface = Mujoco(robot_config, dt=0.001) 22 | interface.connect() 23 | 24 | # instantiate controller 25 | kp = 100 26 | kv = np.sqrt(kp) 27 | ctrlr = Joint( 28 | robot_config, 29 | kp=kp, 30 | kv=kv, 31 | quaternions=[True, True], 32 | ) 33 | 34 | # set up lists for tracking data 35 | q_track = [] 36 | target_track = [] 37 | error_track = [] 38 | 39 | green = [0, 0.9, 0, 0.5] 40 | red = [0.9, 0, 0, 0.5] 41 | threshold = 0.002 # threshold distance for being within target before moving on 42 | 43 | # get the end-effector's initial position 44 | np.random.seed(0) 45 | target_quaternions = [ 46 | np.hstack( 47 | [ 48 | transformations.unit_vector(transformations.random_quaternion()) 49 | for jj in range(2) 50 | ] 51 | ) 52 | for ii in range(4) 53 | ] 54 | 55 | target_index = 0 56 | target = target_quaternions[target_index] 57 | print(target) 58 | target_xyz = robot_config.Tx("EE", q=target) 59 | interface.set_mocap_xyz(name="target", xyz=target_xyz) 60 | 61 | try: 62 | # get the end-effector's initial position 63 | feedback = interface.get_feedback() 64 | start = robot_config.Tx("EE", feedback["q"]) 65 | 66 | count = 0.0 67 | print("\nSimulation starting...\n") 68 | while 1: 69 | if glfw.window_should_close(interface.viewer.window): 70 | break 71 | # get joint angle and velocity feedback 72 | feedback = interface.get_feedback() 73 | 74 | # calculate the control signal 75 | u = ctrlr.generate( 76 | q=feedback["q"], 77 | dq=feedback["dq"], 78 | target=target, 79 | ) 80 | 81 | # send forces into Mujoco, step the sim forward 82 | interface.send_forces(u) 83 | 84 | # track data 85 | q_track.append(np.copy(feedback["q"])) 86 | target_track.append(np.copy(target)) 87 | 88 | # calculate the distance between quaternions 89 | error = 0.0 90 | for ii in range(2): 91 | temp = quaternion_multiply( 92 | target[ii * 4 : (ii * 4) + 4], 93 | quaternion_conjugate(feedback["q"][ii * 4 : (ii * 4) + 4]), 94 | ) 95 | temp = 2 * np.arctan2(np.linalg.norm(temp[1:]) * -np.sign(temp[0]), temp[0]) 96 | # quaternion distance for same angles can be 0 or 2*pi, so use a sine 97 | # wave here so 0 and 2*np.pi = 0 98 | error += np.sin(temp / 2) 99 | error_track.append(np.copy(error)) 100 | 101 | if abs(error) < threshold: 102 | interface.model.geom("target").rgba = green 103 | count += 1 104 | else: 105 | count = 0 106 | interface.model.geom("target").rgba = red 107 | 108 | if count >= 1000: 109 | target_index += 1 110 | target = target_quaternions[target_index % len(target_quaternions)] 111 | target_xyz = robot_config.Tx("EE", q=target) 112 | interface.set_mocap_xyz(name="target", xyz=target_xyz) 113 | count = 0 114 | 115 | except: 116 | print(traceback.format_exc()) 117 | 118 | finally: 119 | # stop and reset the Mujoco simulation 120 | interface.disconnect() 121 | 122 | print("Simulation terminated...") 123 | 124 | q_track = np.array(q_track) 125 | target_track = np.array(target_track) 126 | 127 | if q_track.shape[0] > 0: 128 | # plot distance from target and 3D trajectory 129 | import matplotlib.pyplot as plt 130 | from mpl_toolkits.mplot3d import axes3d # pylint: disable=W0611 131 | 132 | plt.figure(figsize=(8, 6)) 133 | plt.ylabel("Distance (m)") 134 | plt.xlabel("Time (ms)") 135 | plt.title("Distance to target") 136 | plt.plot(np.array(error_track)) 137 | plt.show() 138 | -------------------------------------------------------------------------------- /examples/Mujoco/force_osc_abg.py: -------------------------------------------------------------------------------- 1 | """ 2 | Running operational space control using Mujoco. The controller will 3 | move the end-effector to the target object's orientation. 4 | """ 5 | import sys 6 | 7 | import glfw 8 | import numpy as np 9 | 10 | from abr_control.arms.mujoco_config import MujocoConfig as arm 11 | from abr_control.controllers import OSC, Damping 12 | from abr_control.interfaces.mujoco import Mujoco 13 | from abr_control.utils import transformations 14 | 15 | if len(sys.argv) > 1: 16 | arm_model = sys.argv[1] 17 | else: 18 | arm_model = "jaco2" 19 | # initialize our robot config for the jaco2 20 | robot_config = arm(arm_model) 21 | 22 | # create our interface 23 | interface = Mujoco(robot_config, dt=0.001) 24 | interface.connect() 25 | interface.send_target_angles(robot_config.START_ANGLES) 26 | 27 | # damp the movements of the arm 28 | damping = Damping(robot_config, kv=10) 29 | # create opreational space controller 30 | ctrlr = OSC( 31 | robot_config, 32 | kp=200, # position gain 33 | ko=200, # orientation gain 34 | null_controllers=[damping], 35 | # control (alpha, beta, gamma) out of [x, y, z, alpha, beta, gamma] 36 | ctrlr_dof=[False, False, False, True, True, True], 37 | ) 38 | 39 | # set up lists for tracking data 40 | ee_angles_track = [] 41 | target_angles_track = [] 42 | 43 | 44 | try: 45 | print("\nSimulation starting...\n") 46 | cnt = 0 47 | while 1: 48 | if cnt % 1000 == 0: 49 | # generate a random target orientation 50 | rand_orient = transformations.random_quaternion() 51 | print("New target orientation: ", rand_orient) 52 | 53 | if glfw.window_should_close(interface.viewer.window): 54 | break 55 | 56 | # get arm feedback 57 | feedback = interface.get_feedback() 58 | hand_xyz = robot_config.Tx("EE", feedback["q"]) 59 | 60 | # set our target to our ee_xyz since we are only focussing on orinetation 61 | interface.set_mocap_xyz("target_orientation", hand_xyz) 62 | interface.set_mocap_orientation("target_orientation", rand_orient) 63 | 64 | target = np.hstack( 65 | [ 66 | interface.get_xyz("target_orientation"), 67 | transformations.euler_from_quaternion( 68 | interface.get_orientation("target_orientation"), "rxyz" 69 | ), 70 | ] 71 | ) 72 | 73 | rc_matrix = robot_config.R("EE", feedback["q"]) 74 | rc_angles = transformations.euler_from_matrix(rc_matrix, axes="rxyz") 75 | 76 | u = ctrlr.generate( 77 | q=feedback["q"], 78 | dq=feedback["dq"], 79 | target=target, 80 | ) 81 | 82 | # apply the control signal, step the sim forward 83 | interface.send_forces(u) 84 | 85 | # track data 86 | ee_angles_track.append( 87 | transformations.euler_from_matrix( 88 | robot_config.R("EE", feedback["q"]), axes="rxyz" 89 | ) 90 | ) 91 | target_angles_track.append( 92 | transformations.euler_from_quaternion( 93 | interface.get_orientation("target_orientation"), "rxyz" 94 | ) 95 | ) 96 | cnt += 1 97 | 98 | finally: 99 | # stop and reset the simulation 100 | interface.disconnect() 101 | 102 | print("Simulation terminated...") 103 | 104 | ee_angles_track = np.array(ee_angles_track) 105 | target_angles_track = np.array(target_angles_track) 106 | 107 | if ee_angles_track.shape[0] > 0: 108 | # plot distance from target and 3D trajectory 109 | import matplotlib.pyplot as plt 110 | 111 | plt.figure() 112 | plt.plot(ee_angles_track) 113 | plt.gca().set_prop_cycle(None) 114 | plt.plot(target_angles_track, "--") 115 | plt.ylabel("3D orientation (rad)") 116 | plt.xlabel("Time (s)") 117 | plt.tight_layout() 118 | plt.show() 119 | -------------------------------------------------------------------------------- /examples/Mujoco/force_osc_xyz.py: -------------------------------------------------------------------------------- 1 | """ 2 | Move the jao2 Mujoco arm to a target position. 3 | The simulation ends after 1500 time steps, and the 4 | trajectory of the end-effector is plotted in 3D. 5 | """ 6 | import sys 7 | import traceback 8 | 9 | import glfw 10 | import numpy as np 11 | 12 | from abr_control.arms.mujoco_config import MujocoConfig as arm 13 | from abr_control.controllers import OSC, Damping 14 | from abr_control.interfaces.mujoco import Mujoco 15 | from abr_control.utils import transformations 16 | 17 | if len(sys.argv) > 1: 18 | arm_model = sys.argv[1] 19 | else: 20 | arm_model = "jaco2" 21 | # initialize our robot config for the jaco2 22 | robot_config = arm(arm_model) 23 | 24 | # create our Mujoco interface 25 | dt = 0.001 26 | interface = Mujoco(robot_config, dt=dt) 27 | interface.connect(joint_names=[f"joint{ii}" for ii in range(len(robot_config.START_ANGLES))]) 28 | interface.send_target_angles(robot_config.START_ANGLES) 29 | 30 | # damp the movements of the arm 31 | damping = Damping(robot_config, kv=10) 32 | # instantiate controller 33 | ctrlr = OSC( 34 | robot_config, 35 | kp=200, 36 | null_controllers=[damping], 37 | vmax=[0.5, 0], # [m/s, rad/s] 38 | # control (x, y, z) out of [x, y, z, alpha, beta, gamma] 39 | ctrlr_dof=[True, True, True, False, False, False], 40 | ) 41 | 42 | # set up lists for tracking data 43 | ee_track = [] 44 | target_track = [] 45 | 46 | target_geom = "target" 47 | green = [0, 0.9, 0, 0.5] 48 | red = [0.9, 0, 0, 0.5] 49 | 50 | np.random.seed(0) 51 | def gen_target(interface): 52 | target_xyz = (np.random.rand(3) + np.array([-0.5, -0.5, 0.5])) * np.array( 53 | [1, 1, 0.5] 54 | ) 55 | interface.set_mocap_xyz(name="target", xyz=target_xyz) 56 | 57 | try: 58 | # get the end-effector's initial position 59 | feedback = interface.get_feedback() 60 | start = robot_config.Tx("EE", feedback["q"]) 61 | 62 | # make the target offset from that start position 63 | gen_target(interface) 64 | 65 | count = 0 66 | print("\nSimulation starting...\n") 67 | while 1: 68 | if glfw.window_should_close(interface.viewer.window): 69 | break 70 | # get joint angle and velocity feedback 71 | feedback = interface.get_feedback() 72 | 73 | target = np.hstack( 74 | [ 75 | interface.get_xyz("target"), 76 | transformations.euler_from_quaternion( 77 | interface.get_orientation("target"), "rxyz" 78 | ), 79 | ] 80 | ) 81 | 82 | # calculate the control signal 83 | u = ctrlr.generate( 84 | q=feedback["q"], 85 | dq=feedback["dq"], 86 | target=target, 87 | ) 88 | 89 | # send forces into Mujoco, step the sim forward 90 | interface.send_forces(u) 91 | 92 | # calculate end-effector position 93 | ee_xyz = robot_config.Tx("EE", q=feedback["q"]) 94 | # track data 95 | ee_track.append(np.copy(ee_xyz)) 96 | target_track.append(np.copy(target[:3])) 97 | 98 | error = np.linalg.norm(ee_xyz - target[:3]) 99 | if error < 0.02: 100 | # interface.model.geom(target_geom).rgba = green 101 | count += 1 102 | else: 103 | count = 0 104 | # interface.model.geom(target_geom).rgba = red 105 | 106 | if count >= 50: 107 | print("Generating a new target") 108 | gen_target(interface) 109 | count = 0 110 | 111 | except: 112 | print(traceback.format_exc()) 113 | 114 | finally: 115 | # stop and reset the Mujoco simulation 116 | interface.disconnect() 117 | 118 | print("Simulation terminated...") 119 | 120 | ee_track = np.array(ee_track) 121 | target_track = np.array(target_track) 122 | 123 | if ee_track.shape[0] > 0: 124 | # plot distance from target and 3D trajectory 125 | import matplotlib.pyplot as plt 126 | from mpl_toolkits.mplot3d import axes3d # pylint: disable=W0611 127 | 128 | fig = plt.figure(figsize=(8, 12)) 129 | ax1 = fig.add_subplot(211) 130 | ax1.set_ylabel("Distance (m)") 131 | ax1.set_xlabel("Time (ms)") 132 | ax1.set_title("Distance to target") 133 | ax1.plot( 134 | np.sqrt(np.sum((np.array(target_track) - np.array(ee_track)) ** 2, axis=1)) 135 | ) 136 | 137 | ax2 = fig.add_subplot(212, projection="3d") 138 | ax2.set_title("End-Effector Trajectory") 139 | ax2.plot(ee_track[:, 0], ee_track[:, 1], ee_track[:, 2], label="ee_xyz") 140 | ax2.scatter( 141 | target_track[1, 0], 142 | target_track[1, 1], 143 | target_track[1, 2], 144 | label="target", 145 | c="r", 146 | ) 147 | ax2.scatter( 148 | ee_track[0, 0], 149 | ee_track[0, 1], 150 | ee_track[0, 2], 151 | label="start", 152 | c="g", 153 | ) 154 | ax2.legend() 155 | plt.show() 156 | -------------------------------------------------------------------------------- /examples/Mujoco/force_osc_xyz_balljoint.py: -------------------------------------------------------------------------------- 1 | """ 2 | Move the jao2 Mujoco arm to a target position. 3 | The simulation ends after 1500 time steps, and the 4 | trajectory of the end-effector is plotted in 3D. 5 | """ 6 | import traceback 7 | 8 | import glfw 9 | import numpy as np 10 | 11 | from abr_control.arms.mujoco_config import MujocoConfig as arm 12 | from abr_control.controllers import OSC 13 | from abr_control.interfaces.mujoco import Mujoco 14 | from abr_control.utils import transformations 15 | 16 | # initialize our robot config for the jaco2 17 | robot_config = arm("mujoco_balljoint.xml", folder=".") 18 | 19 | # create our Mujoco interface 20 | interface = Mujoco(robot_config, dt=0.001) 21 | interface.connect() 22 | 23 | # instantiate controller 24 | ctrlr = OSC( 25 | robot_config, 26 | kp=200, 27 | # control (x, y, z) out of [x, y, z, alpha, beta, gamma] 28 | ctrlr_dof=[True, True, True, False, False, False], 29 | ) 30 | 31 | # set up lists for tracking data 32 | ee_track = [] 33 | target_track = [] 34 | 35 | green = [0, 0.9, 0, 0.5] 36 | red = [0.9, 0, 0, 0.5] 37 | 38 | # get the end-effector's initial position 39 | targets = [ 40 | np.array([-0.3, -0.3, 0.9]), 41 | np.array([0.3, -0.3, 0.9]), 42 | np.array([0.3, 0.3, 0.9]), 43 | np.array([-0.3, 0.3, 0.9]), 44 | ] 45 | target_index = 0 46 | interface.set_mocap_xyz(name="target", xyz=targets[0]) 47 | 48 | try: 49 | # get the end-effector's initial position 50 | feedback = interface.get_feedback() 51 | start = robot_config.Tx("EE", feedback["q"]) 52 | 53 | count = 0.0 54 | print("\nSimulation starting...\n") 55 | while 1: 56 | if glfw.window_should_close(interface.viewer.window): 57 | break 58 | # get joint angle and velocity feedback 59 | feedback = interface.get_feedback() 60 | 61 | target = np.hstack( 62 | [ 63 | interface.get_xyz("target"), 64 | transformations.euler_from_quaternion( 65 | interface.get_orientation("target"), "rxyz" 66 | ), 67 | ] 68 | ) 69 | 70 | # calculate the control signal 71 | u = ctrlr.generate( 72 | q=feedback["q"], 73 | dq=feedback["dq"], 74 | target=target, 75 | ) 76 | 77 | # send forces into Mujoco, step the sim forward 78 | interface.send_forces(u) 79 | 80 | # calculate end-effector position 81 | ee_xyz = robot_config.Tx("EE", q=feedback["q"]) 82 | # track data 83 | ee_track.append(np.copy(ee_xyz)) 84 | target_track.append(np.copy(target[:3])) 85 | 86 | error = np.linalg.norm(ee_xyz - target[:3]) 87 | if error < 0.02: 88 | interface.model.geom("target").rgba = green 89 | count += 1 90 | else: 91 | count = 0 92 | interface.model.geom("target").rgba = red 93 | 94 | if count >= 50: 95 | target_index += 1 96 | interface.set_mocap_xyz( 97 | name="target", xyz=targets[target_index % len(targets)] 98 | ) 99 | count = 0 100 | 101 | except: 102 | print(traceback.format_exc()) 103 | 104 | finally: 105 | # stop and reset the Mujoco simulation 106 | interface.disconnect() 107 | 108 | print("Simulation terminated...") 109 | 110 | ee_track = np.array(ee_track) 111 | target_track = np.array(target_track) 112 | 113 | if ee_track.shape[0] > 0: 114 | # plot distance from target and 3D trajectory 115 | import matplotlib.pyplot as plt 116 | from mpl_toolkits.mplot3d import axes3d # pylint: disable=W0611 117 | 118 | fig = plt.figure(figsize=(8, 12)) 119 | ax1 = fig.add_subplot(211) 120 | ax1.set_ylabel("Distance (m)") 121 | ax1.set_xlabel("Time (ms)") 122 | ax1.set_title("Distance to target") 123 | ax1.plot( 124 | np.sqrt(np.sum((np.array(target_track) - np.array(ee_track)) ** 2, axis=1)) 125 | ) 126 | 127 | ax2 = fig.add_subplot(212, projection="3d") 128 | ax2.set_title("End-Effector Trajectory") 129 | ax2.plot(ee_track[:, 0], ee_track[:, 1], ee_track[:, 2], label="ee_xyz") 130 | ax2.scatter( 131 | target_track[0, 0], 132 | target_track[0, 1], 133 | target_track[0, 2], 134 | label="target", 135 | c="r", 136 | ) 137 | ax2.legend() 138 | plt.show() 139 | -------------------------------------------------------------------------------- /examples/Mujoco/force_osc_xyz_geometric_arm.py: -------------------------------------------------------------------------------- 1 | """ 2 | Minimal example using the mujoco interface with a three link arm 3 | reaching to a target with three degrees of freedom 4 | 5 | To plot the joint angles call the script with True 6 | passed through sys.argv 7 | python threelink.py True 8 | """ 9 | import sys 10 | 11 | import glfw 12 | import matplotlib.pyplot as plt 13 | import numpy as np 14 | from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 15 | 16 | from abr_control.arms.mujoco_config import MujocoConfig 17 | from abr_control.controllers import OSC 18 | from abr_control.interfaces.mujoco import Mujoco 19 | 20 | print( 21 | "****************************************************************" 22 | + "***************************************" 23 | ) 24 | print( 25 | "\n***Append 1, 2, or 3 to your function call to change between a" 26 | + " onejoint, twojoint, or threejoint arm***\n" 27 | ) 28 | print( 29 | "****************************************************************" 30 | + "***************************************" 31 | ) 32 | show_plot = False 33 | if len(sys.argv) > 1: 34 | N_JOINTS = int(sys.argv[1]) 35 | else: 36 | N_JOINTS = 3 37 | 38 | if N_JOINTS == 1: 39 | model_filename = "onejoint" 40 | ctrlr_dof = [True, False, False, False, False, False] 41 | elif N_JOINTS == 2: 42 | model_filename = "twojoint" 43 | ctrlr_dof = [True, True, False, False, False, False] 44 | elif N_JOINTS == 3: 45 | model_filename = "threejoint" 46 | ctrlr_dof = [True, True, True, False, False, False] 47 | else: 48 | raise Exception("Only 1-3 joint arms are available in this example") 49 | 50 | 51 | robot_config = MujocoConfig(model_filename) 52 | 53 | dt = 0.001 54 | interface = Mujoco(robot_config, dt=dt) 55 | interface.connect() 56 | interface.send_target_angles(robot_config.START_ANGLES) 57 | 58 | ctrlr = OSC(robot_config, kp=10, kv=5, ctrlr_dof=ctrlr_dof) 59 | 60 | interface.send_target_angles(np.ones(N_JOINTS)) 61 | 62 | target = np.array([0.1, 0.1, 0.3, 0, 0, 0]) 63 | interface.set_mocap_xyz("target", target[:3]) 64 | 65 | interface.set_mocap_xyz("hand", np.array([0.2, 0.4, 1])) 66 | 67 | q_track = [] 68 | count = 0 69 | 70 | link_name = "EE" 71 | try: 72 | while True: 73 | 74 | if glfw.window_should_close(interface.viewer.window): 75 | break 76 | 77 | feedback = interface.get_feedback() 78 | hand_xyz = robot_config.Tx(link_name) 79 | u = ctrlr.generate( 80 | q=feedback["q"], 81 | dq=feedback["dq"], 82 | target=target, 83 | ref_frame=link_name, 84 | ) 85 | interface.send_forces(u) 86 | 87 | if np.linalg.norm(hand_xyz[:N_JOINTS] - target[:N_JOINTS]) < 0.01: 88 | target[0] = np.random.uniform(0.2, 0.25) * np.sign(np.random.uniform(-1, 1)) 89 | target[1] = np.random.uniform(0.2, 0.25) * np.sign(np.random.uniform(-1, 1)) 90 | target[2] = np.random.uniform(0.4, 0.5) 91 | interface.set_mocap_xyz("target", target[:3]) 92 | 93 | q_track.append(feedback["q"]) 94 | count += 1 95 | 96 | finally: 97 | interface.disconnect() 98 | 99 | q_track = np.asarray(q_track) 100 | 101 | if show_plot: 102 | plt.figure(figsize=(30, 30)) 103 | 104 | plt.plot(q_track) 105 | plt.ylabel("Joint Angles [rad]") 106 | plt.legend(["0", "1"]) 107 | 108 | plt.show() 109 | -------------------------------------------------------------------------------- /examples/Mujoco/force_osc_xyz_linear_path_gaussian_velocity.py: -------------------------------------------------------------------------------- 1 | """ 2 | Running operational space control using Mujoco. The controller will 3 | move the end-effector to the target object's X position and orientation. 4 | 5 | The cartesian direction being controlled is set in the first three booleans 6 | of the ctrlr_dof parameter 7 | """ 8 | import sys 9 | 10 | import glfw 11 | import numpy as np 12 | 13 | from abr_control.arms.mujoco_config import MujocoConfig as arm 14 | from abr_control.controllers import OSC, Damping 15 | from abr_control.controllers.path_planners import PathPlanner 16 | from abr_control.controllers.path_planners.position_profiles import Linear 17 | from abr_control.controllers.path_planners.velocity_profiles import Gaussian 18 | from abr_control.interfaces.mujoco import Mujoco 19 | 20 | max_a = 2 21 | n_targets = 100 22 | 23 | if len(sys.argv) > 1: 24 | arm_model = sys.argv[1] 25 | else: 26 | arm_model = "jaco2" 27 | # initialize our robot config for the jaco2 28 | robot_config = arm(arm_model) 29 | 30 | ctrlr_dof = [True, True, True, False, False, False] 31 | dof_labels = ["x", "y", "z", "a", "b", "g"] 32 | dof_print = f"* DOF Controlled: {np.array(dof_labels)[ctrlr_dof]} *" 33 | stars = "*" * len(dof_print) 34 | print(stars) 35 | print(dof_print) 36 | print(stars) 37 | dt = 0.001 38 | 39 | # create our interface 40 | interface = Mujoco(robot_config, dt=dt) 41 | interface.connect() 42 | interface.send_target_angles(robot_config.START_ANGLES) 43 | 44 | # damp the movements of the arm 45 | damping = Damping(robot_config, kv=10) 46 | # create opreational space controller 47 | ctrlr = OSC( 48 | robot_config, 49 | kp=30, 50 | kv=20, 51 | ko=180, 52 | null_controllers=[damping], 53 | vmax=[10, 10], # [m/s, rad/s] 54 | # control (x, alpha, beta, gamma) out of [x, y, z, alpha, beta, gamma] 55 | ctrlr_dof=ctrlr_dof, 56 | ) 57 | 58 | path_planner = PathPlanner( 59 | pos_profile=Linear(), vel_profile=Gaussian(dt=dt, acceleration=max_a) 60 | ) 61 | 62 | # set up lists for tracking data 63 | ee_track = [] 64 | target_track = [] 65 | 66 | try: 67 | print("\nSimulation starting...\n") 68 | for ii in range(0, n_targets): 69 | feedback = interface.get_feedback() 70 | hand_xyz = robot_config.Tx("EE", feedback["q"]) 71 | 72 | pos_target = np.array( 73 | [ 74 | np.random.uniform(low=-0.4, high=0.4), 75 | np.random.uniform(low=-0.4, high=0.4), 76 | np.random.uniform(low=0.3, high=0.6), 77 | ] 78 | ) 79 | 80 | path_planner.generate_path( 81 | start_position=hand_xyz, target_position=pos_target, max_velocity=2 82 | ) 83 | 84 | interface.set_mocap_xyz("target", pos_target) 85 | at_target = 0 86 | count = 0 87 | 88 | while at_target < 500: 89 | if count > 5000: 90 | break 91 | filtered_target = path_planner.next() 92 | interface.set_mocap_xyz("target_orientation", filtered_target[:3]) 93 | 94 | feedback = interface.get_feedback() 95 | hand_xyz = robot_config.Tx("EE", feedback["q"]) 96 | 97 | if glfw.window_should_close(interface.viewer.window): 98 | break 99 | 100 | u = ctrlr.generate( 101 | q=feedback["q"], 102 | dq=feedback["dq"], 103 | target=filtered_target, 104 | ) 105 | 106 | # apply the control signal, step the sim forward 107 | interface.send_forces(u) 108 | 109 | # track data 110 | ee_track.append(np.copy(hand_xyz)) 111 | target_track.append(np.copy(pos_target[:3])) 112 | 113 | if np.linalg.norm(hand_xyz - pos_target) < 0.02: 114 | at_target += 1 115 | else: 116 | at_target = 0 117 | count += 1 118 | 119 | finally: 120 | # stop and reset the simulation 121 | interface.disconnect() 122 | 123 | print("Simulation terminated...") 124 | 125 | ee_track = np.array(ee_track) 126 | target_track = np.array(target_track) 127 | 128 | if ee_track.shape[0] > 0: 129 | # plot distance from target and 3D trajectory 130 | import matplotlib.pyplot as plt 131 | from mpl_toolkits.mplot3d import axes3d # pylint: disable=W0611 132 | 133 | fig = plt.figure(figsize=(8, 12)) 134 | ax1 = fig.add_subplot(211) 135 | ax1.set_ylabel("3D position (m)") 136 | for ii, controlled_dof in enumerate(ctrlr_dof[:3]): 137 | if controlled_dof: 138 | ax1.plot(ee_track[:, ii], label=dof_labels[ii]) 139 | ax1.plot(target_track[:, ii], "--") 140 | ax1.legend() 141 | 142 | ax3 = fig.add_subplot(212, projection="3d") 143 | ax3.set_title("End-Effector Trajectory") 144 | ax3.plot(ee_track[:, 0], ee_track[:, 1], ee_track[:, 2], label="ee_xyz") 145 | ax3.scatter( 146 | target_track[0, 0], 147 | target_track[0, 1], 148 | target_track[0, 2], 149 | label="target", 150 | c="g", 151 | ) 152 | ax3.legend() 153 | plt.show() 154 | -------------------------------------------------------------------------------- /examples/Mujoco/mujoco_balljoint.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /examples/Mujoco/mujoco_two_balljoints.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /examples/Mujoco/position_joint_control_inverse_kinematics.py: -------------------------------------------------------------------------------- 1 | """ 2 | Running the joint controller with an inverse kinematics path planner 3 | for a Mujoco simulation. The path planning system will generate 4 | a trajectory in joint space that moves the end effector in a straight line 5 | to the target, which changes every n time steps. 6 | """ 7 | import glfw 8 | import numpy as np 9 | 10 | from abr_control.arms.mujoco_config import MujocoConfig as arm 11 | from abr_control.controllers import path_planners 12 | from abr_control.interfaces.mujoco import Mujoco 13 | from abr_control.utils import transformations 14 | 15 | # initialize our robot config for the jaco2 16 | robot_config = arm("ur5", use_sim_state=False) 17 | 18 | # create our path planner 19 | n_timesteps = 2000 20 | path_planner = path_planners.InverseKinematics(robot_config) 21 | 22 | # create our interface 23 | dt = 0.001 24 | interface = Mujoco(robot_config, dt=dt) 25 | interface.connect() 26 | interface.send_target_angles(robot_config.START_ANGLES) 27 | feedback = interface.get_feedback() 28 | 29 | try: 30 | print("\nSimulation starting...") 31 | print("Click to move the target.\n") 32 | 33 | count = 0 34 | while 1: 35 | if glfw.window_should_close(interface.viewer.window): 36 | break 37 | 38 | if count % n_timesteps == 0: 39 | feedback = interface.get_feedback() 40 | target_xyz = np.array( 41 | [ 42 | np.random.random() * 0.5 - 0.25, 43 | np.random.random() * 0.5 - 0.25, 44 | np.random.random() * 0.5 + 0.5, 45 | ] 46 | ) 47 | R = robot_config.R("EE", q=feedback["q"]) 48 | target_orientation = transformations.euler_from_matrix(R, "sxyz") 49 | # update the position of the target 50 | interface.set_mocap_xyz("target", target_xyz) 51 | 52 | # can use 3 different methods to calculate inverse kinematics 53 | # see inverse_kinematics.py file for details 54 | path_planner.generate_path( 55 | position=feedback["q"], 56 | target_position=np.hstack([target_xyz, target_orientation]), 57 | method=3, 58 | dt=0.005, 59 | n_timesteps=n_timesteps, 60 | plot=False, 61 | ) 62 | 63 | # returns desired [position, velocity] 64 | target = path_planner.next()[0] 65 | 66 | # use position control 67 | interface.send_target_angles(target[: robot_config.N_JOINTS]) 68 | interface.viewer.render() 69 | 70 | count += 1 71 | 72 | finally: 73 | # stop and reset the simulation 74 | interface.disconnect() 75 | 76 | print("Simulation terminated...") 77 | -------------------------------------------------------------------------------- /examples/PyGame/avoid_obstacles.py: -------------------------------------------------------------------------------- 1 | """ 2 | Running operational space control with the PyGame display, using the 3 | obstacle avoidance signal. The obstacle location can be moved 4 | by clicking on the background. 5 | """ 6 | import numpy as np 7 | 8 | from abr_control.arms import threejoint as arm 9 | from abr_control.controllers import OSC, AvoidObstacles, Damping 10 | from abr_control.interfaces.pygame import PyGame 11 | 12 | # initialize our robot config 13 | robot_config = arm.Config() 14 | # create our arm simulation 15 | arm_sim = arm.ArmSim(robot_config) 16 | 17 | avoid = AvoidObstacles(robot_config, threshold=1, gain=30) 18 | # damp the movements of the arm 19 | damping = Damping(robot_config, kv=10) 20 | # create an operational space controller 21 | ctrlr = OSC( 22 | robot_config, 23 | kp=10, 24 | null_controllers=[avoid, damping], 25 | vmax=[10, 0], # [m/s, rad/s] 26 | # control (x, y) out of [x, y, z, alpha, beta, gamma] 27 | ctrlr_dof=[True, True, False, False, False, False], 28 | ) 29 | 30 | 31 | def on_click(self, mouse_x, mouse_y): 32 | self.circles[0][0] = mouse_x 33 | self.circles[0][1] = mouse_y 34 | 35 | 36 | # create our interface 37 | interface = PyGame(robot_config, arm_sim, on_click=on_click) 38 | interface.connect() 39 | 40 | # create an obstacle 41 | interface.add_circle(xyz=[0, 0, 0], radius=0.2) 42 | 43 | # create a target [x, y, z]] 44 | target_xyz = [0, 2, 0] 45 | # create a target orientation [alpha, beta, gamma] 46 | target_angles = [0, 0, 0] 47 | interface.set_target(target_xyz) 48 | 49 | 50 | try: 51 | print("\nSimulation starting...") 52 | print("Click to move the obstacle.\n") 53 | 54 | count = 0 55 | while 1: 56 | # get arm feedback 57 | feedback = interface.get_feedback() 58 | hand_xyz = robot_config.Tx("EE", feedback["q"]) 59 | 60 | target = np.hstack([target_xyz, target_angles]) 61 | # generate an operational space control signal 62 | u = ctrlr.generate( 63 | q=feedback["q"], 64 | dq=feedback["dq"], 65 | target=target, 66 | ) 67 | 68 | # add in obstacle avoidance 69 | obs_xy = interface.get_mousexy() 70 | if obs_xy is not None: 71 | avoid.set_obstacles(obstacles=[[obs_xy[0], obs_xy[1], 0, 0.2]]) 72 | 73 | # apply the control signal, step the sim forward 74 | interface.send_forces(u, update_display=(count % 20 == 0)) 75 | 76 | # change target location once hand is within 77 | # 5mm of the target 78 | if np.sqrt(np.sum((target_xyz - hand_xyz) ** 2)) < 0.005: 79 | target_xyz = np.array( 80 | [np.random.random() * 2 - 1, np.random.random() * 2 + 1, 0] 81 | ) 82 | # update the position of the target 83 | interface.set_target(target_xyz) 84 | 85 | count += 1 86 | 87 | finally: 88 | # stop and reset the simulation 89 | interface.disconnect() 90 | 91 | print("Simulation terminated...") 92 | -------------------------------------------------------------------------------- /examples/PyGame/force_osc_g.py: -------------------------------------------------------------------------------- 1 | """ 2 | Running operational space control with the PyGame display. The arm will 3 | move the end-effector to a target orientation, which can be changed by 4 | pressing the left/right arrow keys. 5 | """ 6 | from os import environ 7 | 8 | import numpy as np 9 | 10 | environ["PYGAME_HIDE_SUPPORT_PROMPT"] = "1" 11 | import pygame 12 | 13 | from abr_control.arms import threejoint as arm 14 | from abr_control.controllers import OSC, Damping 15 | 16 | # from abr_control.arms import twojoint as arm 17 | from abr_control.interfaces.pygame import PyGame 18 | from abr_control.utils import transformations 19 | 20 | # initialize our robot config 21 | robot_config = arm.Config() 22 | # create our arm simulation 23 | arm_sim = arm.ArmSim(robot_config) 24 | 25 | # damp the movements of the arm 26 | damping = Damping(robot_config, kv=10) 27 | # create operational space controller 28 | ctrlr = OSC( 29 | robot_config, 30 | kp=50, 31 | null_controllers=[damping], 32 | # control (gamma) out of [x, y, z, alpha, beta, gamma] 33 | ctrlr_dof=[False, False, False, False, False, True], 34 | ) 35 | 36 | 37 | def on_keypress(self, key): 38 | if key == pygame.K_LEFT: 39 | self.theta += np.pi / 10 40 | if key == pygame.K_RIGHT: 41 | self.theta -= np.pi / 10 42 | print("theta: ", self.theta) 43 | 44 | # set the target orientation to be the initial EE 45 | # orientation rotated by theta 46 | R_theta = np.array( 47 | [ 48 | [np.cos(interface.theta), -np.sin(interface.theta), 0], 49 | [np.sin(interface.theta), np.cos(interface.theta), 0], 50 | [0, 0, 1], 51 | ] 52 | ) 53 | R_target = np.dot(R_theta, R) 54 | self.target_angles = transformations.euler_from_matrix(R_target, axes="sxyz") 55 | 56 | 57 | # create our interface 58 | interface = PyGame(robot_config, arm_sim, dt=0.001, on_keypress=on_keypress) 59 | interface.connect() 60 | interface.theta = -3 * np.pi / 4 61 | feedback = interface.get_feedback() 62 | R = robot_config.R("EE", feedback["q"]) 63 | interface.on_keypress(interface, None) 64 | 65 | 66 | try: 67 | print("\nSimulation starting...") 68 | print("Press left or right arrow to change target orientation angle.\n") 69 | 70 | count = 0 71 | while 1: 72 | # get arm feedback 73 | feedback = interface.get_feedback() 74 | hand_xyz = robot_config.Tx("EE", feedback["q"]) 75 | 76 | target = np.hstack([np.zeros(3), interface.target_angles]) 77 | u = ctrlr.generate( 78 | q=feedback["q"], 79 | dq=feedback["dq"], 80 | target=target, 81 | ) 82 | 83 | # apply the control signal, step the sim forward 84 | interface.send_forces(u, update_display=(count % 20 == 0)) 85 | 86 | count += 1 87 | 88 | finally: 89 | # stop and reset the simulation 90 | interface.disconnect() 91 | 92 | print("Simulation terminated...") 93 | -------------------------------------------------------------------------------- /examples/PyGame/force_osc_xy.py: -------------------------------------------------------------------------------- 1 | """ 2 | Running operational space control with the PyGame display. The arm will 3 | move the end-effector to the target, which can be moved by 4 | clicking on the background. 5 | """ 6 | import numpy as np 7 | 8 | from abr_control.arms import threejoint as arm 9 | from abr_control.controllers import OSC, Damping, RestingConfig 10 | 11 | # from abr_control.arms import twojoint as arm 12 | from abr_control.interfaces.pygame import PyGame 13 | 14 | # initialize our robot config 15 | robot_config = arm.Config() 16 | # create our arm simulation 17 | arm_sim = arm.ArmSim(robot_config) 18 | 19 | # damp the movements of the arm 20 | damping = Damping(robot_config, kv=10) 21 | # keep the arm near a default configuration 22 | resting_config = RestingConfig( 23 | robot_config, kp=50, kv=np.sqrt(50), rest_angles=[np.pi / 4, np.pi, None] 24 | ) 25 | 26 | # create an operational space controller 27 | ctrlr = OSC( 28 | robot_config, 29 | kp=20, 30 | use_C=True, 31 | null_controllers=[damping, resting_config], 32 | # control (x, y) out of [x, y, z, alpha, beta, gamma] 33 | ctrlr_dof=[True, True, False, False, False, False], 34 | ) 35 | 36 | 37 | def on_click(self, mouse_x, mouse_y): 38 | self.target[0] = self.mouse_x 39 | self.target[1] = self.mouse_y 40 | 41 | 42 | # create our interface 43 | interface = PyGame(robot_config, arm_sim, dt=0.001, on_click=on_click) 44 | interface.connect() 45 | 46 | # create a target 47 | feedback = interface.get_feedback() 48 | target_xyz = robot_config.Tx("EE", feedback["q"]) 49 | interface.set_target(target_xyz) 50 | 51 | 52 | try: 53 | print("\nSimulation starting...") 54 | print("Click to move the target.\n") 55 | 56 | count = 0 57 | while 1: 58 | # get arm feedback 59 | feedback = interface.get_feedback() 60 | hand_xyz = robot_config.Tx("EE", feedback["q"]) 61 | 62 | target = np.hstack([target_xyz, np.zeros(3)]) 63 | # generate an operational space control signal 64 | u = ctrlr.generate( 65 | q=feedback["q"], 66 | dq=feedback["dq"], 67 | target=target, 68 | ) 69 | 70 | new_target = interface.get_mousexy() 71 | if new_target is not None: 72 | target_xyz[0:2] = new_target 73 | interface.set_target(target_xyz) 74 | 75 | # apply the control signal, step the sim forward 76 | interface.send_forces(u, update_display=(count % 50 == 0)) 77 | 78 | count += 1 79 | 80 | finally: 81 | # stop and reset the simulation 82 | interface.disconnect() 83 | 84 | print("Simulation terminated...") 85 | -------------------------------------------------------------------------------- /examples/PyGame/force_osc_xy_avoid_joint_limits.py: -------------------------------------------------------------------------------- 1 | """ 2 | Running operational space control with the PyGame display, using an exponential 3 | additive signal when to push away from joints. 4 | The target location can be moved by clicking on the background. 5 | """ 6 | import numpy as np 7 | 8 | from abr_control.arms import threejoint as arm 9 | from abr_control.controllers import OSC, AvoidJointLimits, Damping 10 | 11 | # from abr_control.arms import twojoint as arm 12 | from abr_control.interfaces.pygame import PyGame 13 | 14 | print("\nClick to move the target.\n") 15 | 16 | # initialize our robot config 17 | robot_config = arm.Config() 18 | # create our arm simulation 19 | arm_sim = arm.ArmSim(robot_config) 20 | 21 | avoid = AvoidJointLimits( 22 | robot_config, 23 | min_joint_angles=[np.pi / 5.0] * robot_config.N_JOINTS, 24 | max_joint_angles=[np.pi / 2.0] * robot_config.N_JOINTS, 25 | max_torque=[100.0] * robot_config.N_JOINTS, 26 | ) 27 | # damp the movements of the arm 28 | damping = Damping(robot_config, kv=10) 29 | # create an operational space controller 30 | ctrlr = OSC( 31 | robot_config, 32 | kp=100, 33 | null_controllers=[avoid, damping], 34 | # control (x, y) out of [x, y, z, alpha, beta, gamma] 35 | ctrlr_dof=[True, True, False, False, False, False], 36 | ) 37 | 38 | 39 | def on_click(self, mouse_x, mouse_y): 40 | self.target[0] = self.mouse_x 41 | self.target[1] = self.mouse_y 42 | 43 | 44 | # create our interface 45 | interface = PyGame( 46 | robot_config, 47 | arm_sim, 48 | dt=0.001, 49 | on_click=on_click, 50 | q_init=[np.pi / 4, np.pi / 2, np.pi / 2], 51 | ) 52 | interface.connect() 53 | 54 | # create a target [x, y, z]] 55 | target_xyz = [0, 2, 0] 56 | # create a target orientation [alpha, beta, gamma] 57 | target_angles = [0, 0, 0] 58 | interface.set_target(target_xyz) 59 | 60 | 61 | try: 62 | print("\nSimulation starting...\n") 63 | 64 | count = 0 65 | while 1: 66 | # get arm feedback 67 | feedback = interface.get_feedback() 68 | hand_xyz = robot_config.Tx("EE", feedback["q"]) 69 | 70 | target = np.hstack([target_xyz, target_angles]) 71 | # generate an operational space control signal 72 | u = ctrlr.generate( 73 | q=feedback["q"], 74 | dq=feedback["dq"], 75 | target=target, 76 | ) 77 | 78 | new_target_xy = interface.get_mousexy() 79 | if new_target_xy is not None: 80 | target_xyz[:2] = new_target_xy 81 | interface.set_target(target_xyz) 82 | 83 | # apply the control signal, step the sim forward 84 | interface.send_forces(u, update_display=(count % 20 == 0)) 85 | 86 | count += 1 87 | 88 | finally: 89 | # stop and reset the simulation 90 | interface.disconnect() 91 | 92 | print("Simulation terminated...") 93 | -------------------------------------------------------------------------------- /examples/PyGame/force_osc_xy_dynamics_adaptation.py: -------------------------------------------------------------------------------- 1 | """ 2 | Running operational space control with nonlinear adaptation using the 3 | PyGame display. The controller works to drive the arm's end-effector to 4 | the target while an unexpected external force is applied. Target position 5 | can be by clicking inside the display. 6 | To turn adaptation on or off, press the spacebar. 7 | """ 8 | from os import environ 9 | 10 | import numpy as np 11 | 12 | environ["PYGAME_HIDE_SUPPORT_PROMPT"] = "1" 13 | import pygame 14 | 15 | from abr_control.arms import threejoint as arm 16 | from abr_control.controllers import OSC, Damping, signals 17 | 18 | # from abr_control.arms import twojoint as arm 19 | from abr_control.interfaces.pygame import PyGame 20 | 21 | # initialize our robot config 22 | robot_config = arm.Config() 23 | # create our arm simulation 24 | arm_sim = arm.ArmSim(robot_config) 25 | 26 | # damp the movements of the arm 27 | damping = Damping(robot_config, kv=10) 28 | # create an operational space controller 29 | ctrlr = OSC( 30 | robot_config, 31 | kp=50, 32 | null_controllers=[damping], 33 | # control (x, y) out of [x, y, z, alpha, beta, gamma] 34 | ctrlr_dof=[True, True, False, False, False, False], 35 | ) 36 | 37 | # create our nonlinear adaptive controller 38 | adapt = signals.DynamicsAdaptation( 39 | n_input=robot_config.N_JOINTS, 40 | n_output=robot_config.N_JOINTS, 41 | pes_learning_rate=1e-4, 42 | means=[0, 0, 0], 43 | variances=[np.pi, np.pi, np.pi], 44 | ) 45 | 46 | 47 | def on_click(self, mouse_x, mouse_y): 48 | self.target[0] = self.mouse_x 49 | self.target[1] = self.mouse_y 50 | 51 | 52 | def on_keypress(self, key): 53 | if key == pygame.K_SPACE: 54 | self.adaptation = not self.adaptation 55 | print("adaptation: ", self.adaptation) 56 | 57 | 58 | # create our interface 59 | interface = PyGame(robot_config, arm_sim, on_click=on_click, on_keypress=on_keypress) 60 | interface.connect() 61 | interface.adaptation = False # set adaptation False to start 62 | 63 | # create a target 64 | feedback = interface.get_feedback() 65 | target_xyz = robot_config.Tx("EE", feedback["q"]) 66 | target_angles = np.zeros(3) 67 | interface.set_target(target_xyz) 68 | 69 | # get Jacobians to each link for calculating perturbation 70 | J_links = [ 71 | robot_config._calc_J(f"link{ii}", x=[0, 0, 0]) for ii in range(robot_config.N_LINKS) 72 | ] 73 | 74 | 75 | try: 76 | print("\nSimulation starting...") 77 | print("Click to move the target.") 78 | print("Press space to turn on adaptation.\n") 79 | 80 | count = 0 81 | while 1: 82 | # get arm feedback 83 | feedback = interface.get_feedback() 84 | hand_xyz = robot_config.Tx("EE", feedback["q"]) 85 | 86 | target = np.hstack([target_xyz, target_angles]) 87 | # generate an operational space control signal 88 | u = ctrlr.generate( 89 | q=feedback["q"], 90 | dq=feedback["dq"], 91 | target=target, 92 | ) 93 | 94 | # if adaptation is on (toggled with space bar) 95 | if interface.adaptation: 96 | u += adapt.generate( 97 | input_signal=feedback["q"], training_signal=ctrlr.training_signal 98 | ) 99 | 100 | fake_gravity = np.array([[0, -9.81, 0, 0, 0, 0]]).T * 10.0 101 | g = np.zeros((robot_config.N_JOINTS, 1)) 102 | for ii in range(robot_config.N_LINKS): 103 | pars = tuple(feedback["q"]) + tuple([0, 0, 0]) 104 | g += np.dot(J_links[ii](*pars).T, fake_gravity) 105 | u += g.squeeze() 106 | 107 | new_target = interface.get_mousexy() 108 | if new_target is not None: 109 | target_xyz[:2] = new_target 110 | interface.set_target(target_xyz) 111 | 112 | # apply the control signal, step the sim forward 113 | interface.send_forces(u, update_display=(count % 20 == 0)) 114 | 115 | count += 1 116 | 117 | finally: 118 | # stop and reset the simulation 119 | interface.disconnect() 120 | 121 | print("Simulation terminated...") 122 | -------------------------------------------------------------------------------- /examples/PyGame/force_osc_xy_integrated_error.py: -------------------------------------------------------------------------------- 1 | """ 2 | Running operational space control with nonlinear adaptation using the 3 | PyGame display. The controller works to drive the arm's end-effector to 4 | the target while an unexpected external force is applied. Target position 5 | can be by clicking inside the display. 6 | To turn adaptation on or off, press the spacebar. 7 | """ 8 | from os import environ 9 | 10 | import numpy as np 11 | 12 | environ["PYGAME_HIDE_SUPPORT_PROMPT"] = "1" 13 | 14 | from abr_control.arms import threejoint as arm 15 | from abr_control.controllers import OSC, Damping 16 | 17 | # from abr_control.arms import twojoint as arm 18 | from abr_control.interfaces.pygame import PyGame 19 | 20 | # initialize our robot config 21 | robot_config = arm.Config() 22 | # create our arm simulation 23 | arm_sim = arm.ArmSim(robot_config) 24 | 25 | # damp the movements of the arm 26 | damping = Damping(robot_config, kv=10) 27 | # create an operational space controller 28 | ctrlr = OSC( 29 | robot_config, 30 | kp=50, 31 | ki=1e-3, 32 | null_controllers=[damping], 33 | # control (x, y) out of [x, y, z, alpha, beta, gamma] 34 | ctrlr_dof=[True, True, False, False, False, False], 35 | ) 36 | 37 | 38 | def on_click(self, mouse_x, mouse_y): 39 | self.target[0] = self.mouse_x 40 | self.target[1] = self.mouse_y 41 | 42 | 43 | # create our interface 44 | interface = PyGame(robot_config, arm_sim, on_click=on_click) 45 | interface.connect() 46 | 47 | # create a target 48 | feedback = interface.get_feedback() 49 | target_xyz = robot_config.Tx("EE", feedback["q"]) 50 | target_angles = np.zeros(3) 51 | interface.set_target(target_xyz) 52 | 53 | # get Jacobians to each link for calculating perturbation 54 | J_links = [ 55 | robot_config._calc_J(f"link{ii}", x=[0, 0, 0]) for ii in range(robot_config.N_LINKS) 56 | ] 57 | 58 | 59 | try: 60 | print("\nSimulation starting...") 61 | print("Click to move the target.") 62 | 63 | count = 0 64 | while 1: 65 | # get arm feedback 66 | feedback = interface.get_feedback() 67 | hand_xyz = robot_config.Tx("EE", feedback["q"]) 68 | 69 | target = np.hstack([target_xyz, target_angles]) 70 | # generate an operational space control signal 71 | u = ctrlr.generate( 72 | q=feedback["q"], 73 | dq=feedback["dq"], 74 | target=target, 75 | ) 76 | 77 | fake_gravity = np.array([[0, -9.81, 0, 0, 0, 0]]).T * 10.0 78 | g = np.zeros((robot_config.N_JOINTS, 1)) 79 | for ii in range(robot_config.N_LINKS): 80 | pars = tuple(feedback["q"]) + tuple([0, 0, 0]) 81 | g += np.dot(J_links[ii](*pars).T, fake_gravity) 82 | u += g.squeeze() 83 | 84 | new_target = interface.get_mousexy() 85 | if new_target is not None: 86 | target_xyz[:2] = new_target 87 | interface.set_target(target_xyz) 88 | 89 | # apply the control signal, step the sim forward 90 | interface.send_forces(u, update_display=(count % 20 == 0)) 91 | 92 | count += 1 93 | 94 | finally: 95 | # stop and reset the simulation 96 | interface.disconnect() 97 | 98 | print("Simulation terminated...") 99 | -------------------------------------------------------------------------------- /examples/PyGame/force_osc_xy_linear_position_linear_velocity.py: -------------------------------------------------------------------------------- 1 | """ 2 | Running the operational space control with a first order path planner 3 | using the PyGame display. The path planner system will generate a 4 | trajectory for the controller to follow, moving the end-effector in a 5 | straight line to the target, which changes every n time steps. 6 | """ 7 | import matplotlib.pyplot as plt 8 | import numpy as np 9 | 10 | from abr_control.arms import threejoint as arm 11 | from abr_control.controllers import OSC, Damping 12 | from abr_control.controllers.path_planners import PathPlanner 13 | from abr_control.controllers.path_planners.position_profiles import Linear as LinearP 14 | from abr_control.controllers.path_planners.velocity_profiles import Linear as LinearV 15 | 16 | # from abr_control.arms import twojoint as arm 17 | from abr_control.interfaces.pygame import PyGame 18 | 19 | dt = 0.001 20 | # initialize our robot config 21 | robot_config = arm.Config() 22 | 23 | # create our arm simulation 24 | arm_sim = arm.ArmSim(robot_config) 25 | 26 | # damp the movements of the arm 27 | damping = Damping(robot_config, kv=10) 28 | # create an operational space controller 29 | ctrlr = OSC( 30 | robot_config, 31 | kp=200, 32 | null_controllers=[damping], 33 | # control (gamma) out of [x, y, z, alpha, beta, gamma] 34 | ctrlr_dof=[True, True, False, False, False, False], 35 | ) 36 | 37 | # create our path planner 38 | target_dx = 0.001 39 | path_planner = PathPlanner( 40 | pos_profile=LinearP(), vel_profile=LinearV(dt=dt, acceleration=1) 41 | ) 42 | 43 | 44 | # create our interface 45 | interface = PyGame(robot_config, arm_sim, dt=dt) 46 | interface.connect() 47 | first_pass = True 48 | 49 | 50 | try: 51 | print("\nSimulation starting...") 52 | print("Click to move the target.\n") 53 | 54 | count = 0 55 | dx_track = [] 56 | # number of additional steps too allow for a reach 57 | buffer_steps = 100 58 | while 1: 59 | # get arm feedback 60 | feedback = interface.get_feedback() 61 | hand_xyz = robot_config.Tx("EE", feedback["q"]) 62 | dx_track.append( 63 | np.linalg.norm(np.dot(robot_config.J("EE", feedback["q"]), feedback["dq"])) 64 | ) 65 | 66 | if count - buffer_steps == path_planner.n_timesteps or first_pass: 67 | count = 0 68 | first_pass = False 69 | target_xyz = np.array( 70 | [np.random.random() * 2 - 1, np.random.random() * 2 + 1, 0] 71 | ) 72 | # update the position of the target 73 | interface.set_target(target_xyz) 74 | path_planner.generate_path( 75 | start_position=hand_xyz, 76 | target_position=target_xyz, 77 | start_velocity=1, 78 | target_velocity=1, 79 | max_velocity=1, 80 | plot=False, 81 | ) 82 | 83 | # returns desired [position, velocity] 84 | target = path_planner.next() 85 | 86 | # generate an operational space control signal 87 | u = ctrlr.generate(q=feedback["q"], dq=feedback["dq"], target=target) 88 | 89 | # apply the control signal, step the sim forward 90 | interface.send_forces(u, update_display=(count % 20 == 0)) 91 | 92 | count += 1 93 | 94 | finally: 95 | # stop and reset the simulation 96 | interface.disconnect() 97 | 98 | dx_track = np.array(dx_track) * path_planner.dt 99 | plt.plot(dx_track, lw=2, label="End-effector velocity") 100 | plt.plot(np.ones(dx_track.shape) * target_dx, "r--", lw=2, label="Target velocity") 101 | plt.ylabel("Velocity (m/s)") 102 | plt.xlabel("Time (s)") 103 | plt.legend() 104 | plt.tight_layout() 105 | plt.show() 106 | 107 | print("Simulation terminated...") 108 | -------------------------------------------------------------------------------- /examples/PyGame/force_osc_xyg.py: -------------------------------------------------------------------------------- 1 | """ 2 | Running operational space control with the PyGame display. The arm will 3 | move the end-effector to the target at a specified orientation around 4 | the z axis. The (x, y) target can be changed by clicking on the background, 5 | and the target orientation can be changed with the left/right arrow keys. 6 | """ 7 | from os import environ 8 | 9 | import numpy as np 10 | 11 | environ["PYGAME_HIDE_SUPPORT_PROMPT"] = "1" 12 | import pygame 13 | 14 | from abr_control.arms import threejoint as arm 15 | from abr_control.controllers import OSC, Damping 16 | 17 | # from abr_control.arms import twojoint as arm 18 | from abr_control.interfaces.pygame import PyGame 19 | from abr_control.utils import transformations 20 | 21 | # initialize our robot config 22 | robot_config = arm.Config() 23 | # create our arm simulation 24 | arm_sim = arm.ArmSim(robot_config) 25 | 26 | # damp the movements of the arm 27 | damping = Damping(robot_config, kv=10) 28 | # create operational space controller 29 | ctrlr = OSC( 30 | robot_config, 31 | kp=50, 32 | null_controllers=[damping], 33 | # control (x, y, gamma) out of [x, y, z, alpha, beta, gamma] 34 | ctrlr_dof=[True, True, False, False, False, True], 35 | ) 36 | 37 | 38 | def on_click(self, mouse_x, mouse_y): 39 | self.target[0] = self.mouse_x 40 | self.target[1] = self.mouse_y 41 | 42 | 43 | def on_keypress(self, key): 44 | if key == pygame.K_LEFT: 45 | self.theta += np.pi / 10 46 | if key == pygame.K_RIGHT: 47 | self.theta -= np.pi / 10 48 | print("theta: ", self.theta) 49 | 50 | # set the target orientation to be the initial EE 51 | # orientation rotated by theta 52 | R_theta = np.array( 53 | [ 54 | [np.cos(interface.theta), -np.sin(interface.theta), 0], 55 | [np.sin(interface.theta), np.cos(interface.theta), 0], 56 | [0, 0, 1], 57 | ] 58 | ) 59 | R_target = np.dot(R_theta, R) 60 | self.target_angles = transformations.euler_from_matrix(R_target, axes="sxyz") 61 | 62 | 63 | # create our interface 64 | interface = PyGame( 65 | robot_config, arm_sim, dt=0.001, on_click=on_click, on_keypress=on_keypress 66 | ) 67 | interface.connect() 68 | feedback = interface.get_feedback() 69 | # set target position 70 | target_xyz = robot_config.Tx("EE", feedback["q"]) 71 | interface.set_target(target_xyz) 72 | # set target orientation 73 | interface.theta = -3 * np.pi / 4 74 | R = robot_config.R("EE", feedback["q"]) 75 | interface.on_keypress(interface, None) 76 | 77 | 78 | try: 79 | print("\nSimulation starting...") 80 | print("Click to move the target.") 81 | print("Press left or right arrow to change target orientation angle.\n") 82 | 83 | count = 0 84 | while 1: 85 | # get arm feedback 86 | feedback = interface.get_feedback() 87 | hand_xyz = robot_config.Tx("EE", feedback["q"]) 88 | 89 | target = np.hstack([target_xyz, interface.target_angles]) 90 | u = ctrlr.generate( 91 | q=feedback["q"], 92 | dq=feedback["dq"], 93 | target=target, 94 | ) 95 | 96 | new_target = interface.get_mousexy() 97 | if new_target is not None: 98 | target_xyz[0:2] = new_target 99 | interface.set_target(target_xyz) 100 | 101 | # apply the control signal, step the sim forward 102 | interface.send_forces(u, update_display=(count % 20 == 0)) 103 | 104 | count += 1 105 | 106 | finally: 107 | # stop and reset the simulation 108 | interface.disconnect() 109 | 110 | print("Simulation terminated...") 111 | -------------------------------------------------------------------------------- /examples/PyGame/force_sliding_xy.py: -------------------------------------------------------------------------------- 1 | """ 2 | Running sliding control with the PyGame display. The arm will 3 | move the end-effector to the target, which can be moved by 4 | clicking on the background. 5 | """ 6 | from abr_control.arms import threejoint as arm 7 | from abr_control.controllers import Sliding 8 | 9 | # from abr_control.arms import twojoint as arm 10 | from abr_control.interfaces.pygame import PyGame 11 | 12 | # initialize our robot config 13 | robot_config = arm.Config() 14 | # create our arm simulation 15 | arm_sim = arm.ArmSim(robot_config) 16 | 17 | # create an operational space controller 18 | ctrlr = Sliding(robot_config) 19 | 20 | 21 | def on_click(self, mouse_x, mouse_y): 22 | self.target[0] = self.mouse_x 23 | self.target[1] = self.mouse_y 24 | 25 | 26 | # create our interface 27 | interface = PyGame(robot_config, arm_sim, dt=0.001, on_click=on_click) 28 | interface.connect() 29 | 30 | # create a target 31 | feedback = interface.get_feedback() 32 | target_xyz = robot_config.Tx("EE", feedback["q"]) 33 | interface.set_target(target_xyz) 34 | 35 | 36 | try: 37 | print("\nSimulation starting...") 38 | print("Click to move the target.\n") 39 | 40 | count = 0 41 | while 1: 42 | # get arm feedback 43 | feedback = interface.get_feedback() 44 | hand_xyz = robot_config.Tx("EE", feedback["q"]) 45 | 46 | # generate an operational space control signal 47 | u = ctrlr.generate(q=feedback["q"], dq=feedback["dq"], target=target_xyz) 48 | 49 | new_target = interface.get_mousexy() 50 | if new_target is not None: 51 | target_xyz[0:2] = new_target 52 | interface.set_target(target_xyz) 53 | 54 | # apply the control signal, step the sim forward 55 | interface.send_forces(u, update_display=(count % 20 == 0)) 56 | 57 | count += 1 58 | 59 | finally: 60 | # stop and reset the simulation 61 | interface.disconnect() 62 | 63 | print("Simulation terminated...") 64 | -------------------------------------------------------------------------------- /examples/PyGame/force_sliding_xy_dynamics_adaptation.py: -------------------------------------------------------------------------------- 1 | """ 2 | Running sliding control with nonlinear adaptation using the 3 | PyGame display. The controller works to drive the arm's end-effector to 4 | the target while an unexpected external force is applied. Target position 5 | can be by clicking inside the display. 6 | To turn adaptation on or off, press the spacebar. 7 | """ 8 | from os import environ 9 | 10 | import numpy as np 11 | 12 | environ["PYGAME_HIDE_SUPPORT_PROMPT"] = "1" 13 | import pygame 14 | 15 | from abr_control.arms import threejoint as arm 16 | from abr_control.controllers import Sliding, signals 17 | 18 | # from abr_control.arms import twojoint as arm 19 | from abr_control.interfaces.pygame import PyGame 20 | 21 | # initialize our robot config 22 | robot_config = arm.Config() 23 | # create our arm simulation 24 | arm_sim = arm.ArmSim(robot_config) 25 | 26 | # create an operational space controller 27 | ctrlr = Sliding(robot_config, kd=30) 28 | 29 | # create our nonlinear adaptation controller 30 | adapt = signals.DynamicsAdaptation( 31 | n_input=robot_config.N_JOINTS, 32 | n_output=robot_config.N_JOINTS, 33 | pes_learning_rate=3e-3, 34 | means=[0, 0, 0], 35 | variances=[2 * np.pi, 2 * np.pi, 2 * np.pi], 36 | ) 37 | 38 | 39 | def on_click(self, mouse_x, mouse_y): 40 | self.target[0] = self.mouse_x 41 | self.target[1] = self.mouse_y 42 | 43 | 44 | def on_keypress(self, key): 45 | if key == pygame.K_SPACE: 46 | self.adaptation = not self.adaptation 47 | print("adaptation: ", self.adaptation) 48 | 49 | 50 | # create our interface 51 | interface = PyGame(robot_config, arm_sim, on_click=on_click, on_keypress=on_keypress) 52 | interface.connect() 53 | interface.adaptation = False # set adaptation False to start 54 | 55 | # create a target 56 | feedback = interface.get_feedback() 57 | target_xyz = robot_config.Tx("EE", feedback["q"]) 58 | interface.set_target(target_xyz) 59 | 60 | # get Jacobians to each link for calculating perturbation 61 | J_links = [ 62 | robot_config._calc_J(f"link{ii}", x=[0, 0, 0]) for ii in range(robot_config.N_LINKS) 63 | ] 64 | 65 | 66 | try: 67 | print("\nSimulation starting...") 68 | print("Click to move the target.") 69 | print("Press space to turn on adaptation.\n") 70 | 71 | count = 0 72 | while 1: 73 | # get arm feedback 74 | feedback = interface.get_feedback() 75 | hand_xyz = robot_config.Tx("EE", feedback["q"]) 76 | 77 | # generate an operational space control signal 78 | u = ctrlr.generate(q=feedback["q"], dq=feedback["dq"], target=target_xyz) 79 | 80 | # if adaptation is on (toggled with space bar) 81 | if interface.adaptation: 82 | sig = adapt.generate(input_signal=feedback["q"], training_signal=-ctrlr.s) 83 | u += sig 84 | 85 | fake_gravity = np.array([[0, -9.81, 0, 0, 0, 0]]).T * 10.0 86 | g = np.zeros((robot_config.N_JOINTS, 1)) 87 | for ii in range(robot_config.N_LINKS): 88 | pars = tuple(feedback["q"]) + tuple([0, 0, 0]) 89 | g += np.dot(J_links[ii](*pars).T, fake_gravity) 90 | u += g.squeeze() 91 | 92 | new_target = interface.get_mousexy() 93 | if new_target is not None: 94 | target_xyz[:2] = new_target 95 | interface.set_target(target_xyz) 96 | 97 | # apply the control signal, step the sim forward 98 | interface.send_forces(u, update_display=(count % 20 == 0)) 99 | 100 | count += 1 101 | 102 | finally: 103 | # stop and reset the simulation 104 | interface.disconnect() 105 | 106 | print("Simulation terminated...") 107 | -------------------------------------------------------------------------------- /examples/PyGame/position_joint_control_inverse_kinematics.py: -------------------------------------------------------------------------------- 1 | """ 2 | Running the joint controller with an inverse kinematics path planner 3 | using the PyGame display. The path planning system will generate 4 | a trajectory in joint space that moves the end effector in a straight line 5 | to the target, which changes every n time steps. 6 | """ 7 | import numpy as np 8 | 9 | from abr_control.arms import threejoint as arm 10 | from abr_control.controllers import Joint, path_planners 11 | 12 | # from abr_control.arms import twojoint as arm 13 | from abr_control.interfaces.pygame import PyGame 14 | from abr_control.utils import transformations 15 | 16 | # change this flag to False to use position control 17 | use_force_control = True 18 | 19 | # initialize our robot config 20 | robot_config = arm.Config() 21 | 22 | # create our arm simulation 23 | arm_sim = arm.ArmSim(robot_config) 24 | 25 | if use_force_control: 26 | # create an operational space controller 27 | ctrlr = Joint(robot_config, kp=300, kv=20) 28 | 29 | # create our path planner 30 | n_timesteps = 2000 31 | path_planner = path_planners.InverseKinematics(robot_config) 32 | 33 | # create our interface 34 | dt = 0.001 35 | interface = PyGame(robot_config, arm_sim, dt=dt) 36 | interface.connect() 37 | feedback = interface.get_feedback() 38 | 39 | try: 40 | print("\nSimulation starting...") 41 | print("Click to move the target.\n") 42 | 43 | count = 0 44 | while 1: 45 | # get arm feedback 46 | feedback = interface.get_feedback() 47 | hand_xyz = robot_config.Tx("EE", feedback["q"]) 48 | 49 | if count % n_timesteps == 0: 50 | target_xyz = np.array( 51 | [np.random.random() * 2 - 1, np.random.random() * 2 + 1, 0] 52 | ) 53 | R = robot_config.R("EE", q=feedback["q"]) 54 | target_orientation = transformations.euler_from_matrix(R, "sxyz") 55 | # update the position of the target 56 | interface.set_target(target_xyz) 57 | 58 | # can use 3 different methods to calculate inverse kinematics 59 | # see inverse_kinematics.py file for details 60 | path_planner.generate_path( 61 | position=feedback["q"], 62 | target_position=np.hstack([target_xyz, target_orientation]), 63 | method=3, 64 | dt=0.005, 65 | n_timesteps=n_timesteps, 66 | plot=False, 67 | ) 68 | 69 | # returns desired [position, velocity] 70 | target, _ = path_planner.next() 71 | 72 | if use_force_control: 73 | # generate an operational space control signal 74 | u = ctrlr.generate( 75 | q=feedback["q"], 76 | dq=feedback["dq"], 77 | target=target, 78 | ) 79 | 80 | # apply the control signal, step the sim forward 81 | interface.send_forces(u, update_display=(count % 20 == 0)) 82 | else: 83 | # use position control 84 | interface.send_target_angles( 85 | target[: robot_config.N_JOINTS], 86 | update_display=(count % 20 == 0), 87 | ) 88 | 89 | count += 1 90 | 91 | finally: 92 | # stop and reset the simulation 93 | interface.disconnect() 94 | 95 | print("Simulation terminated...") 96 | -------------------------------------------------------------------------------- /examples/ellipse_position_profile.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abr/abr_control/0f6e47641d9299422fc24e09ee1445877c235cdc/examples/ellipse_position_profile.png -------------------------------------------------------------------------------- /examples/linear_path_gauss_velocity.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abr/abr_control/0f6e47641d9299422fc24e09ee1445877c235cdc/examples/linear_path_gauss_velocity.png -------------------------------------------------------------------------------- /examples/linear_path_linear_velocity.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abr/abr_control/0f6e47641d9299422fc24e09ee1445877c235cdc/examples/linear_path_linear_velocity.png -------------------------------------------------------------------------------- /examples/linear_position_profile.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abr/abr_control/0f6e47641d9299422fc24e09ee1445877c235cdc/examples/linear_position_profile.png -------------------------------------------------------------------------------- /examples/path_planning/ellipse_position_linear_velocity.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from abr_control.controllers.path_planners import PathPlanner 4 | from abr_control.controllers.path_planners.position_profiles import Ellipse 5 | from abr_control.controllers.path_planners.velocity_profiles import Linear 6 | 7 | Pprof = Ellipse(horz_stretch=3) 8 | Vprof = Linear(dt=0.001, acceleration=1) 9 | 10 | path = PathPlanner(pos_profile=Pprof, vel_profile=Vprof, verbose=True) 11 | path.generate_path( 12 | start_position=np.zeros(3), 13 | target_position=np.array([5, 3, -2]), 14 | start_orientation=np.array([0, 0, 0]), 15 | target_orientation=np.array([0, 0, 3.14]), 16 | max_velocity=2, 17 | start_velocity=0, 18 | target_velocity=0, 19 | plot=True, 20 | ) 21 | -------------------------------------------------------------------------------- /examples/path_planning/from_points_position_gauss_velocity.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from abr_control.controllers.path_planners import PathPlanner 4 | from abr_control.controllers.path_planners.position_profiles import FromPoints 5 | from abr_control.controllers.path_planners.velocity_profiles import Gaussian 6 | 7 | pts = np.array( 8 | [ 9 | [0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0], 10 | [0, 0.1, 0.15, 0.2, 0.25, 0.5, 0.75, 0.8, 0.85, 0.9, 1.0], 11 | [0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0], 12 | ] 13 | ) 14 | t = np.linspace(0, 1, pts.shape[1]) 15 | Pprof = FromPoints(pts=pts, x=t) 16 | Vprof = Gaussian(dt=0.001, acceleration=1) 17 | 18 | path = PathPlanner(pos_profile=Pprof, vel_profile=Vprof, verbose=True) 19 | path.generate_path( 20 | start_position=np.zeros(3), 21 | target_position=np.array([5, 3, -2]), 22 | start_orientation=np.array([0, 0, 0]), 23 | target_orientation=np.array([0, 0, 3.14]), 24 | max_velocity=2, 25 | start_velocity=0, 26 | target_velocity=0, 27 | plot=True, 28 | ) 29 | -------------------------------------------------------------------------------- /examples/path_planning/linear_position_gauss_velocity.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from abr_control.controllers.path_planners import PathPlanner 4 | from abr_control.controllers.path_planners.position_profiles import Linear 5 | from abr_control.controllers.path_planners.velocity_profiles import Gaussian 6 | 7 | Pprof = Linear() 8 | Vprof = Gaussian(dt=0.001, acceleration=1) 9 | 10 | path = PathPlanner(pos_profile=Pprof, vel_profile=Vprof, verbose=True) 11 | path.generate_path( 12 | start_position=np.zeros(3), 13 | target_position=np.array([5, 3, -2]), 14 | start_orientation=np.array([0, 0, 0]), 15 | target_orientation=np.array([0, 0, 3.14]), 16 | max_velocity=20, 17 | start_velocity=0, 18 | target_velocity=0, 19 | plot=True, 20 | ) 21 | -------------------------------------------------------------------------------- /examples/path_planning/linear_position_gauss_velocity_successive_target.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example of moving between several randomly generated points where we stop at 3 | each target (v=0) 4 | """ 5 | import matplotlib.pyplot as plt 6 | import numpy as np 7 | 8 | from abr_control.controllers.path_planners import PathPlanner 9 | from abr_control.controllers.path_planners.position_profiles import Linear 10 | from abr_control.controllers.path_planners.velocity_profiles import Gaussian 11 | 12 | n_targets = 3 13 | dt = 0.001 14 | np.random.seed(12) 15 | Pprof = Linear() 16 | Vprof = Gaussian(dt=dt, acceleration=1) 17 | 18 | path_planner = PathPlanner(pos_profile=Pprof, vel_profile=Vprof, verbose=True) 19 | start = np.zeros(6) 20 | targets = np.random.uniform(low=-15, high=15, size=(n_targets, 3)) 21 | yaws = np.random.uniform(low=-3.14, high=3.14, size=n_targets) 22 | 23 | for ii, target in enumerate(targets): 24 | if ii == 0: 25 | start_position = start[:3] 26 | start_orientation = start[3:] 27 | else: 28 | start_position = targets[ii - 1] 29 | start_orientation = [0, 0, yaws[ii - 1]] 30 | 31 | path = path_planner.generate_path( 32 | start_position=start_position, 33 | target_position=target, 34 | max_velocity=6, 35 | start_orientation=start_orientation, 36 | target_orientation=[0, 0, yaws[ii]], 37 | # plot=True 38 | ) 39 | 40 | if ii == 0: 41 | position_path = path[:, :3] 42 | velocity_path = path[:, 3:6] 43 | orientation_path = path[:, 6:9] 44 | angvel_path = path[:, 9:] 45 | elif ii > 0: 46 | position_path = np.vstack((position_path, path[:, :3])) 47 | velocity_path = np.vstack((velocity_path, path[:, 3:6])) 48 | orientation_path = np.vstack((orientation_path, path[:, 6:9])) 49 | angvel_path = np.vstack((angvel_path, path[:, 9:])) 50 | 51 | times = np.arange(0, position_path.shape[0] * dt, dt) 52 | 53 | plt.rcParams.update({"font.size": 16}) 54 | plt.figure(figsize=(15, 6)) 55 | plt.suptitle("Reference Trajectory") 56 | plt.subplot(221) 57 | plt.title("Position Path") 58 | plt.plot(times, position_path) 59 | plt.legend(["x", "y", "z"], loc=1) 60 | plt.ylabel("Position [m]") 61 | plt.xlabel("Time [sec]") 62 | 63 | plt.subplot(222) 64 | plt.title("Velocity Path") 65 | plt.plot(times, velocity_path) 66 | plt.plot(times, np.linalg.norm(velocity_path, axis=1)) 67 | plt.legend(["dx", "dy", "dz", "norm"], loc=1) 68 | plt.ylabel("Velocity [m/s]") 69 | plt.xlabel("Time [sec]") 70 | 71 | plt.subplot(223) 72 | plt.title("Orientation Path") 73 | plt.plot(times, orientation_path) 74 | plt.legend(["pitch", "roll", "yaw"], loc=1) 75 | plt.ylabel("Orientation [rad]") 76 | plt.xlabel("Time [sec]") 77 | 78 | plt.subplot(224) 79 | plt.title("Angular Velocity Path") 80 | plt.plot(times, angvel_path) 81 | plt.legend(["dpitch", "droll", "dyaw"], loc=1) 82 | plt.ylabel("Angular Velocity [rad/s]") 83 | plt.xlabel("Time [sec]") 84 | 85 | plt.tight_layout() 86 | plt.show() 87 | -------------------------------------------------------------------------------- /examples/path_planning/linear_position_linear_velocity.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from abr_control.controllers.path_planners import PathPlanner 4 | from abr_control.controllers.path_planners.position_profiles import Linear as LinPos 5 | from abr_control.controllers.path_planners.velocity_profiles import Linear as LinVel 6 | 7 | Pprof = LinPos() 8 | Vprof = LinVel(dt=0.001, acceleration=1) 9 | 10 | path = PathPlanner(pos_profile=Pprof, vel_profile=Vprof, verbose=True) 11 | path.generate_path( 12 | start_position=np.zeros(3), 13 | target_position=np.array([5, 3, -2]), 14 | start_orientation=np.array([0, 0, 0]), 15 | target_orientation=np.array([0, 0, 3.14]), 16 | max_velocity=2, 17 | start_velocity=0, 18 | target_velocity=0, 19 | plot=True, 20 | ) 21 | -------------------------------------------------------------------------------- /examples/path_planning/non-zero_target_velocity.py: -------------------------------------------------------------------------------- 1 | """ 2 | This script shows an example of planning a path where the final target velocity is 3 | non-zero The path planner will make sure that our path ends at the target position at 4 | the moment it reaches its target velocity. 5 | 6 | When planning successive targets with non-zero target velocities between target 7 | positions, need to make sure these points sit along the same line, if possible, to 8 | reduce any jerk in the velocity path. The path planner specifies a path directly from 9 | start to target position, and only warps it to follow the path profile along that 10 | straight line. When starting at a non-zero velocity, that points along a different 11 | direction (which could happen if the previously planned path ended in a non-zero target 12 | velocity), it's necessary to abruptly adjust those components to move in the desired 13 | direction along the target path. The norm of the velocity will still remain smooth, but 14 | the individual components may jump. 15 | """ 16 | 17 | import matplotlib.pyplot as plt 18 | import numpy as np 19 | 20 | from abr_control.controllers.path_planners import PathPlanner 21 | from abr_control.controllers.path_planners.position_profiles import Linear 22 | from abr_control.controllers.path_planners.velocity_profiles import Gaussian 23 | 24 | Pprof = Linear() 25 | Vprof = Gaussian(dt=0.001, acceleration=1) 26 | 27 | path_planner = PathPlanner(pos_profile=Pprof, vel_profile=Vprof) 28 | start = np.zeros(3) 29 | targets = np.array([[2, 3, 4], [4, 6, 8]]) 30 | 31 | target_velocities = [1, 0.5] 32 | 33 | for ii, target in enumerate(targets): 34 | if ii == 0: 35 | start_position = start 36 | start_velocity = 0 37 | else: 38 | start_position = targets[ii - 1] 39 | start_velocity = target_velocities[ii - 1] 40 | 41 | path = path_planner.generate_path( 42 | start_position=start_position, 43 | target_position=target, 44 | max_velocity=2, 45 | start_velocity=start_velocity, 46 | target_velocity=target_velocities[ii], 47 | ) 48 | 49 | if ii == 0: 50 | position_path = path[:, :3] 51 | velocity_path = path[:, 3:6] 52 | elif ii > 0: 53 | position_path = np.vstack((position_path, path[:, :3])) 54 | velocity_path = np.vstack((velocity_path, path[:, 3:6])) 55 | 56 | plt.figure() 57 | plt.subplot(211) 58 | plt.title("Position Path") 59 | plt.plot(position_path) 60 | 61 | plt.subplot(212) 62 | plt.title("Velocity Path") 63 | plt.plot(velocity_path) 64 | plt.plot(np.linalg.norm(velocity_path, axis=1)) 65 | plt.legend(["dx", "dy", "dz", "norm"]) 66 | plt.show() 67 | -------------------------------------------------------------------------------- /examples/path_planning/sin_position_linear_velocity.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from abr_control.controllers.path_planners import PathPlanner 4 | from abr_control.controllers.path_planners.position_profiles import SinCurve 5 | from abr_control.controllers.path_planners.velocity_profiles import Linear 6 | 7 | Pprof = SinCurve(axes=["x", "z"], n_sample_points=1000, cycles=[1, 1, 2]) 8 | Vprof = Linear(dt=0.001, acceleration=1) 9 | 10 | path = PathPlanner(pos_profile=Pprof, vel_profile=Vprof, verbose=True) 11 | path.generate_path( 12 | start_position=np.zeros(3), 13 | target_position=np.array([5, 3, -2]), 14 | start_orientation=np.array([0, 0, 0]), 15 | target_orientation=np.array([0, 0, 3.14]), 16 | max_velocity=2, 17 | start_velocity=0, 18 | target_velocity=0, 19 | plot=True, 20 | ) 21 | -------------------------------------------------------------------------------- /examples/timing.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abr/abr_control/0f6e47641d9299422fc24e09ee1445877c235cdc/examples/timing.png -------------------------------------------------------------------------------- /examples/timing_plots.py: -------------------------------------------------------------------------------- 1 | import timeit 2 | 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | 6 | from abr_control.arms import jaco2, twojoint, ur5 7 | from abr_control.controllers import OSC 8 | 9 | 10 | def test_timing(arm, config_params, osc_params, use_cython): 11 | robot_config = arm.Config(use_cython=use_cython, **config_params) 12 | ctrlr = OSC(robot_config, **osc_params) 13 | 14 | # then run for timing 15 | n_trials = 1000 16 | times = np.zeros(n_trials + 1) 17 | for ii in range(n_trials + 1): 18 | q = np.random.random(robot_config.N_JOINTS) * 2 * np.pi 19 | dq = np.random.random(robot_config.N_JOINTS) * 5 20 | target = np.random.random(6) * 2 - 1 21 | 22 | start = timeit.default_timer() 23 | ctrlr.generate(q=q, dq=dq, target=target) 24 | 25 | times[ii] = timeit.default_timer() - start 26 | 27 | # strip off first loop to remove load time 28 | times = times[1:] 29 | average_time = np.sum(times) / n_trials 30 | 31 | return average_time 32 | 33 | 34 | param_sets = ( 35 | (twojoint, {}, {}), 36 | (ur5, {}, {"ctrlr_dof": [True] * 6}), 37 | (jaco2, {"hand_attached": False}, {"ctrlr_dof": [True] * 5 + [False]}), 38 | (jaco2, {"hand_attached": True}, {"ctrlr_dof": [True] * 6}), 39 | ) 40 | 41 | timing_tests_python = [] 42 | timing_tests_cython = [] 43 | logs = [timing_tests_python, timing_tests_cython] 44 | for log, use_cython in zip(logs, [False, True]): 45 | for arm, config_params, osc_params in param_sets: 46 | log.append(test_timing(arm, config_params, osc_params, use_cython=use_cython)) 47 | # convert to milliseconds 48 | timing_tests_python = np.array(timing_tests_python) * 1000 49 | timing_tests_cython = np.array(timing_tests_cython) * 1000 50 | 51 | n = len(timing_tests_python) 52 | labels = ["Two joint", "UR5", "Jaco2", "Jaco2 with hand"] 53 | 54 | plt.subplot(2, 1, 1) 55 | python = plt.bar(np.arange(n), timing_tests_python, width=0.5) 56 | plt.title("Timing test for generating OSC signal") 57 | plt.ylabel("Time (ms)") 58 | plt.legend([python], ["Python"], loc=2) 59 | plt.xticks([]) 60 | 61 | plt.subplot(2, 1, 2) 62 | cython = plt.bar(np.arange(n), timing_tests_cython, width=0.5, color="C1") 63 | plt.ylabel("Time (ms)") 64 | plt.legend([cython], ["Cython"], loc=2) 65 | plt.xticks(np.arange(n), labels) 66 | 67 | plt.tight_layout() 68 | plt.show() 69 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | # Automatically generated by nengo-bones, do not edit this file directly 2 | 3 | [build-system] 4 | requires = ["setuptools", "wheel"] 5 | 6 | [tool.black] 7 | target-version = ['py35'] 8 | 9 | [tool.isort] 10 | profile = "black" 11 | src_paths = ["abr_control"] 12 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | # Automatically generated by nengo-bones, do not edit this file directly 2 | 3 | [build_sphinx] 4 | source-dir = docs 5 | build-dir = docs/_build 6 | all_files = 1 7 | 8 | [coverage:run] 9 | source = ./ 10 | 11 | [coverage:report] 12 | # Regexes for lines to exclude from consideration 13 | exclude_lines = 14 | # Have to re-enable the standard pragma 15 | # place ``# pragma: no cover`` at the end of a line to ignore it 16 | pragma: no cover 17 | 18 | # Don't complain if tests don't hit defensive assertion code: 19 | raise NotImplementedError 20 | 21 | # `pass` is just a placeholder, fine if it's not covered 22 | ^[ \t]*pass$ 23 | 24 | 25 | # Patterns for files to exclude from reporting 26 | omit = 27 | */tests/test* 28 | abr_control/_vendor/* 29 | abr_control/interfaces/coppeliasim_files/* 30 | abr_control/arms/threejoint/arm_files/* 31 | 32 | [flake8] 33 | exclude = 34 | __init__.py 35 | abr_control/_vendor/* 36 | abr_control/interfaces/coppeliasim_files/* 37 | abr_control/arms/threejoint/arm_files/* 38 | abr_control/utils/transformations.py 39 | ignore = 40 | E123 41 | E133 42 | E203 43 | E226 44 | E241 45 | E242 46 | E501 47 | E731 48 | F401 49 | W503 50 | C901 51 | E402 52 | E722 53 | max-complexity = 10 54 | max-line-length = 88 55 | 56 | [tool:pytest] 57 | norecursedirs = 58 | .* 59 | *.egg 60 | build 61 | dist 62 | docs 63 | examples 64 | _vendor 65 | xfail_strict = False 66 | 67 | [pylint] 68 | ignore = 69 | _vendor, 70 | arm_files, 71 | coppeliasim_files, 72 | transformations.py, 73 | 74 | [pylint.messages] 75 | disable = 76 | arguments-differ, 77 | assignment-from-no-return, 78 | attribute-defined-outside-init, 79 | bad-continuation, 80 | blacklisted-name, 81 | comparison-with-callable, 82 | duplicate-code, 83 | fixme, 84 | import-error, 85 | invalid-name, 86 | invalid-sequence-index, 87 | len-as-condition, 88 | literal-comparison, 89 | no-else-raise, 90 | no-else-return, 91 | no-member, 92 | no-name-in-module, 93 | no-self-use, 94 | not-an-iterable, 95 | not-context-manager, 96 | protected-access, 97 | redefined-builtin, 98 | stop-iteration-return, 99 | too-few-public-methods, 100 | too-many-arguments, 101 | too-many-branches, 102 | too-many-instance-attributes, 103 | too-many-lines, 104 | too-many-locals, 105 | too-many-return-statements, 106 | too-many-statements, 107 | unexpected-keyword-arg, 108 | unidiomatic-typecheck, 109 | unsubscriptable-object, 110 | unsupported-assignment-operation, 111 | unused-argument, 112 | F0001, 113 | C0116, 114 | C0115, 115 | C0114, 116 | too-many-blank-lines, 117 | W504, 118 | W0621, 119 | W0702, 120 | 121 | [pylint.imports] 122 | known-third-party = 123 | matplotlib, 124 | nengo, 125 | numpy, 126 | pytest, 127 | mpl_toolkits, 128 | nengo, 129 | nengolib, 130 | scipy, 131 | 132 | [pylint.format] 133 | max-line-length = 88 134 | 135 | [pylint.classes] 136 | valid-metaclass-classmethod-first-arg = metacls 137 | 138 | [pylint.reports] 139 | reports = no 140 | score = no 141 | 142 | [codespell] 143 | skip = ./build,*/_build,*-checkpoint.ipynb,./.eggs,./*.egg-info,./.git,*/_vendor,./.mypy_cache, 144 | ignore-words-list = DOF,dof,hist,nd,compiletime 145 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Automatically generated by nengo-bones, do not edit this file directly 4 | 5 | import io 6 | import pathlib 7 | import runpy 8 | 9 | try: 10 | from setuptools import find_packages, setup 11 | except ImportError: 12 | raise ImportError( 13 | "'setuptools' is required but not installed. To install it, " 14 | "follow the instructions at " 15 | "https://pip.pypa.io/en/stable/installing/#installing-with-get-pip-py" 16 | ) 17 | 18 | 19 | def read(*filenames, **kwargs): 20 | encoding = kwargs.get("encoding", "utf-8") 21 | sep = kwargs.get("sep", "\n") 22 | buf = [] 23 | for filename in filenames: 24 | with io.open(filename, encoding=encoding) as f: 25 | buf.append(f.read()) 26 | return sep.join(buf) 27 | 28 | 29 | root = pathlib.Path(__file__).parent 30 | version = runpy.run_path(str(root / "abr_control" / "version.py"))["version"] 31 | 32 | install_req = [ 33 | "cloudpickle>=0.8.0", 34 | "cython>=0.29.0", 35 | "matplotlib>=3.0.0", 36 | "numpy>=1.16.0", 37 | "setuptools>=18.0", 38 | "scipy>=1.1.0", 39 | "sympy>=1.3", 40 | ] 41 | docs_req = [] 42 | optional_req = [ 43 | "mujoco", 44 | "mujoco-python-viewer", 45 | "pyyaml", 46 | ] 47 | tests_req = [ 48 | "pytest>=4.4.0", 49 | "pytest-xdist>=1.26.0", 50 | "pytest-cov>=2.6.0", 51 | "pytest-plt", 52 | "coverage>=4.5.0", 53 | "nengo>=2.8.0", 54 | ] 55 | 56 | setup( 57 | name="abr-control", 58 | version=version, 59 | author="Applied Brain Research", 60 | author_email="travis.dewolf@appliedbrainresearch.com", 61 | packages=find_packages(), 62 | url="https://github.com/abr/abr_control", 63 | include_package_data=False, 64 | license="Free for non-commercial use", 65 | description="Robotic arm control in Python", 66 | long_description=read("README.rst", "CHANGES.rst"), 67 | zip_safe=False, 68 | install_requires=install_req, 69 | extras_require={ 70 | "all": docs_req + optional_req + tests_req, 71 | "docs": docs_req, 72 | "optional": optional_req, 73 | "tests": tests_req, 74 | }, 75 | python_requires=">=3.5", 76 | classifiers=[ 77 | "Development Status :: 5 - Production/Stable", 78 | "Framework :: ABR Control", 79 | "Intended Audience :: Science/Research", 80 | "License :: Free for non-commercial use", 81 | "Operating System :: OS Independent", 82 | "Programming Language :: Python :: 3 :: Only", 83 | "Programming Language :: Python :: 3.6", 84 | "Programming Language :: Python :: 3.7", 85 | "Topic :: Scientific/Engineering :: Artificial Intelligence", 86 | ], 87 | ) 88 | --------------------------------------------------------------------------------