├── .clang-format ├── .codespell_words ├── .devcontainer ├── Dockerfile ├── devcontainer.json └── docker-compose.yml ├── .github └── workflows │ ├── ci.yaml │ ├── docker.yaml │ └── format.yaml ├── .gitignore ├── .pre-commit-config.yaml ├── CHANGELOG.rst ├── CMakeLists.txt ├── CMakePresets.json ├── LICENSE ├── README.md ├── cmake └── FindCatch2.cmake ├── doc ├── INSTALL.md └── USAGE.md ├── include └── pick_ik │ ├── fk_moveit.hpp │ ├── forward_kinematics.hpp │ ├── goal.hpp │ ├── ik_gradient.hpp │ ├── ik_memetic.hpp │ ├── pick_ik_plugin.hpp │ └── robot.hpp ├── package.xml ├── pick_ik_kinematics_description.xml ├── src ├── fk_moveit.cpp ├── forward_kinematics.cpp ├── goal.cpp ├── ik_gradient.cpp ├── ik_memetic.cpp ├── pick_ik_parameters.yaml ├── pick_ik_plugin.cpp └── robot.cpp └── tests ├── CMakeLists.txt ├── goal_tests.cpp ├── ik_memetic_tests.cpp ├── ik_tests.cpp └── robot_tests.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Google 2 | 3 | ColumnLimit: 100 4 | IndentWidth: 4 5 | 6 | BinPackArguments: false 7 | BinPackParameters: false 8 | AllowAllArgumentsOnNextLine: false 9 | AllowAllParametersOfDeclarationOnNextLine: false 10 | 11 | DerivePointerAlignment: false 12 | QualifierAlignment: Right 13 | ReferenceAlignment: Pointer 14 | SpaceAroundPointerQualifiers: Default 15 | PointerAlignment: Left 16 | 17 | IncludeBlocks: Regroup 18 | IncludeCategories: 19 | - Priority: 1 20 | Regex: '^$' 23 | - Priority: 3 24 | Regex: '^<(.+)>$' 25 | -------------------------------------------------------------------------------- /.codespell_words: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/pick_ik/b30065814650fec70e8e5185b9861c2198dfe0c3/.codespell_words -------------------------------------------------------------------------------- /.devcontainer/Dockerfile: -------------------------------------------------------------------------------- 1 | # syntax=docker/dockerfile:1.2 2 | ARG ROS_DISTRO="rolling" 3 | FROM ros:${ROS_DISTRO}-ros-base AS system 4 | # Restate for later use 5 | ARG ROS_DISTRO 6 | RUN rm /ros_entrypoint.sh 7 | 8 | # prevent interactive messages in apt install 9 | ARG DEBIAN_FRONTEND=noninteractive 10 | 11 | # Switch to ros-testing 12 | RUN echo "deb http://packages.ros.org/ros2-testing/ubuntu $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/ros2-latest.list 13 | 14 | # install build dependencies 15 | RUN --mount=type=cache,target=/var/cache/apt,id=apt \ 16 | apt-get update && apt-get upgrade -y --with-new-pkgs \ 17 | && apt-get install -q -y --no-install-recommends \ 18 | build-essential \ 19 | ccache \ 20 | clang-14 \ 21 | clang-format-14 \ 22 | clang-tidy-14 \ 23 | cmake \ 24 | git \ 25 | lld \ 26 | ninja-build \ 27 | openssh-client \ 28 | python3-colcon-common-extensions \ 29 | python3-colcon-mixin \ 30 | python3-pip \ 31 | python3-pytest \ 32 | python3-pytest-cov \ 33 | python3-pytest-repeat \ 34 | python3-pytest-rerunfailures \ 35 | python3-rosdep \ 36 | python3-setuptools \ 37 | python3-vcstool \ 38 | wget \ 39 | && rosdep update \ 40 | && colcon mixin update default \ 41 | && rm -rf /var/lib/apt/lists/* 42 | 43 | # setup ssh 44 | RUN mkdir /root/.ssh \ 45 | && ssh-keyscan -t rsa github.com >> ~/.ssh/known_hosts 46 | 47 | FROM system AS ci 48 | 49 | # install dependencies of this project 50 | WORKDIR /root/ws 51 | COPY ../ ./src/pick_ik 52 | RUN --mount=type=cache,target=/var/cache/apt,id=apt \ 53 | . /opt/ros/$ROS_DISTRO/setup.sh \ 54 | && rosdep update && apt-get update \ 55 | && rosdep install -q -y \ 56 | --from-paths src \ 57 | --ignore-src \ 58 | --rosdistro ${ROS_DISTRO} \ 59 | && rm -rf /var/lib/apt/lists/* 60 | RUN rm -rf /root/ws 61 | 62 | FROM ci AS devcontainer 63 | 64 | ARG USER=dev 65 | ARG UID=1000 66 | ARG GID=$UID 67 | 68 | # Setup user home directory 69 | RUN groupadd --gid $GID $USER \ 70 | && useradd --uid $GID --gid $UID -m $USER --groups sudo \ 71 | && echo $USER ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USER \ 72 | && chmod 0440 /etc/sudoers.d/$USER \ 73 | && echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /home/${USER}/.profile \ 74 | && mkdir -p /home/${USER}/ws \ 75 | && chown -R ${GID}:${UID} /home/${USER} 76 | 77 | # install pre-commit 78 | RUN python3 -m pip install -U \ 79 | pre-commit 80 | 81 | # install development tooling 82 | RUN --mount=type=cache,target=/var/cache/apt,id=apt \ 83 | apt-get update && apt-get upgrade -y \ 84 | && apt-get install -q -y --no-install-recommends \ 85 | gdb \ 86 | lldb-14 \ 87 | && rm -rf /var/lib/apt/lists/* 88 | 89 | USER $USER 90 | ENV SHELL /bin/bash 91 | ENTRYPOINT [] 92 | -------------------------------------------------------------------------------- /.devcontainer/devcontainer.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "Docker Compose", 3 | "dockerComposeFile": [ 4 | "docker-compose.yml" 5 | ], 6 | "remoteEnv": { 7 | "LOCAL_WORKSPACE_FOLDER": "${localWorkspaceFolder}" 8 | }, 9 | "service": "devcontainer", 10 | "workspaceFolder": "/home/dev/pick_ik", 11 | "remoteUser": "dev", 12 | "customizations": { 13 | "vscode": { 14 | "extensions": [ 15 | "ms-azuretools.vscode-docker", 16 | "ms-vscode.cmake-tools", 17 | "vadimcn.vscode-lldb", 18 | "ms-vscode.cpptools-extension-pack", 19 | "elagil.pre-commit-helper", 20 | "matepek.vscode-catch2-test-adapter" 21 | ] 22 | } 23 | }, 24 | "features": { 25 | "ghcr.io/devcontainers/features/github-cli:1": { 26 | "version": "latest" 27 | } 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /.devcontainer/docker-compose.yml: -------------------------------------------------------------------------------- 1 | version: '3.8' 2 | services: 3 | devcontainer: 4 | build: 5 | context: ../ 6 | dockerfile: .devcontainer/Dockerfile 7 | command: sleep infinity 8 | container_name: dev-pick_ik 9 | environment: 10 | - DISPLAY=${DISPLAY} 11 | - QT_X11_NO_MITSHM=1 12 | - ROS_DOMAIN_ID=99 13 | extra_hosts: 14 | - pick_ik:127.0.0.1 15 | hostname: pick_ik 16 | image: pick_ik:latest 17 | network_mode: host 18 | privileged: true 19 | user: dev 20 | volumes: 21 | - ../:/home/dev/pick_ik 22 | - ~/.ssh:/home/dev/.ssh:ro 23 | - ~/.gitconfig:/home/dev/.gitconfig:ro 24 | - ~/.local/.pick_ik/ccache:/home/dev/.ccache 25 | - ~/.local/.pick_ik/log:/home/dev/.ros/log 26 | - /tmp/.X11-unix:/tmp/.X11-unix:ro 27 | - ${SSH_AUTH_SOCK}:${SSH_AUTH_SOCK} 28 | working_dir: /home/pick_ik 29 | -------------------------------------------------------------------------------- /.github/workflows/ci.yaml: -------------------------------------------------------------------------------- 1 | name: ci 2 | 3 | on: 4 | workflow_dispatch: 5 | pull_request: 6 | push: 7 | 8 | jobs: 9 | build-and-test: 10 | runs-on: ubuntu-22.04 11 | container: ghcr.io/picknikrobotics/pick_ik:ci 12 | env: 13 | CCACHE_DIR: ${{ github.workspace }}/.ccache 14 | steps: 15 | - name: Check out the repo 16 | uses: actions/checkout@v4 17 | - name: Cache ccache 18 | uses: pat-s/always-upload-cache@v3.0.11 19 | with: 20 | path: ${{ env.CCACHE_DIR }} 21 | key: ccache-${{ github.sha }}-${{ github.run_id }} 22 | restore-keys: | 23 | ccache-${{ github.sha }} 24 | ccache 25 | - name: Source ROS Workspaces 26 | run: | 27 | . /opt/ros/rolling/setup.sh 28 | echo "$(env)" >> $GITHUB_ENV 29 | - name: Configure 30 | run: cmake --preset relwithdebinfo 31 | - name: Build 32 | working-directory: ./build/relwithdebinfo 33 | run: ninja 34 | - name: Test 35 | working-directory: ./build/relwithdebinfo 36 | run: ctest . --rerun-failed --output-on-failure 37 | -------------------------------------------------------------------------------- /.github/workflows/docker.yaml: -------------------------------------------------------------------------------- 1 | name: docker 2 | 3 | on: 4 | workflow_dispatch: 5 | schedule: 6 | - cron: '0 0 * * 0' 7 | push: 8 | paths: 9 | - '.devcontainer/Dockerfile' 10 | - '.github/workflows/docker.yaml' 11 | - 'package.xml' 12 | 13 | jobs: 14 | ci-docker: 15 | runs-on: ubuntu-22.04 16 | steps: 17 | - name: Check out the repo 18 | uses: actions/checkout@v4 19 | - name: Set up Docker Buildx 20 | id: buildx 21 | uses: docker/setup-buildx-action@v3 22 | - name: Login to Github Container Registry 23 | uses: docker/login-action@v3 24 | with: 25 | registry: ghcr.io 26 | username: ${{ github.repository_owner }} 27 | password: ${{ secrets.GITHUB_TOKEN }} 28 | - name: Build and push 29 | uses: docker/build-push-action@v6 30 | with: 31 | context: . 32 | file: .devcontainer/Dockerfile 33 | push: true 34 | target: ci 35 | tags: ghcr.io/picknikrobotics/pick_ik:ci 36 | -------------------------------------------------------------------------------- /.github/workflows/format.yaml: -------------------------------------------------------------------------------- 1 | name: format 2 | 3 | on: 4 | workflow_dispatch: 5 | pull_request: 6 | push: 7 | 8 | jobs: 9 | pre-commit: 10 | name: pre-commit 11 | runs-on: ubuntu-22.04 12 | steps: 13 | - uses: actions/checkout@v4 14 | - uses: actions/setup-python@v5 15 | - name: Install clang-format-14 16 | run: sudo apt-get install clang-format-14 17 | - uses: pre-commit/action@v3.0.1 18 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | .vscode 3 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | # Standard hooks 3 | - repo: https://github.com/pre-commit/pre-commit-hooks 4 | rev: v5.0.0 5 | hooks: 6 | - id: check-added-large-files 7 | - id: check-ast 8 | - id: check-builtin-literals 9 | - id: check-byte-order-marker 10 | - id: check-case-conflict 11 | - id: check-docstring-first 12 | - id: check-executables-have-shebangs 13 | - id: check-json 14 | - id: check-merge-conflict 15 | - id: check-symlinks 16 | - id: check-toml 17 | - id: check-vcs-permalinks 18 | - id: check-yaml 19 | - id: debug-statements 20 | - id: destroyed-symlinks 21 | - id: detect-private-key 22 | - id: end-of-file-fixer 23 | - id: fix-byte-order-marker 24 | - id: fix-encoding-pragma 25 | - id: forbid-new-submodules 26 | - id: mixed-line-ending 27 | - id: name-tests-test 28 | - id: no-commit-to-branch 29 | - id: requirements-txt-fixer 30 | - id: sort-simple-yaml 31 | - id: trailing-whitespace 32 | 33 | - repo: https://github.com/psf/black 34 | rev: 24.10.0 35 | hooks: 36 | - id: black 37 | 38 | - repo: local 39 | hooks: 40 | - id: clang-format 41 | name: clang-format 42 | description: Format files with ClangFormat 14. 43 | entry: clang-format-14 44 | language: system 45 | files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ 46 | args: ['-fallback-style=none', '-i'] 47 | 48 | - repo: https://github.com/codespell-project/codespell 49 | rev: v2.3.0 50 | hooks: 51 | - id: codespell 52 | args: ['--write-changes', '--ignore-words=.codespell_words'] 53 | exclude: CHANGELOG.rst 54 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package pick_ik 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.1.1 (2024-11-30) 6 | ------------------ 7 | * Initialize with the best seed instead of initial seed at population wipeout (`#77 `_) 8 | * Fix timeout calculation if solution callback fails (`#73 `_) 9 | * Remove incorrect override of tip_frames (`#68 `_) 10 | * Contributors: Amal Nanavati, Sebastian Castro, Timon Engelke 11 | 12 | 1.1.0 (2023-12-13) 13 | ------------------ 14 | * Support continuous (unbounded) joints properly (`#59 `_) 15 | * Run elite gradient descent in separate threads (`#61 `_) 16 | * Contributors: Sebastian Castro 17 | 18 | 1.0.2 (2023-07-25) 19 | ------------------ 20 | * New options to control solution quality and performance 21 | * Option to rerun optimization until valid solution or timeout (`#53 `_) 22 | * Option to keep optimizing after a valid solution was found (`#46 `_) 23 | * Approximate solution cost threshold (`#51 `_) and joint jump threshold parameters (`#42 `_) 24 | * Position scale parameter (`#47 `_) 25 | * Fix ordering of joint limits when loading robot model (`#54 `_) 26 | * Fix use of solution callback (`#48 `_) 27 | * Remove unnecessary preprocessor macro (`#40 `_) 28 | * Contributors: Marc Bestmann, Sebastian Castro, Erik Holum 29 | 30 | 1.0.1 (2023-03-28) 31 | ------------------ 32 | * Set Werror through CMake presets (`#39 `_) 33 | * Replace lower_bounds with gt_eq (`#37 `_) 34 | * Upgrade with new pkgs to fix issue with ROS 35 | * Target include subdirectory 36 | * Update Catch2 version to 3.3.0 37 | * Fix overriding of package 38 | * Fix orientation calculation in cost function and frame tests (`#31 `_) 39 | * Fix orientation calculation 40 | * Update plugin return values 41 | * Remove redundant (and incorrect) joints bounds check 42 | * Use Eigen angular distance calculation 43 | * Small grammar fixes (`#28 `_) 44 | * Contributors: Sebastian Castro, Stephanie Eng, Tyler Weaver 45 | 46 | 1.0.0 (2022-12-08) 47 | ------------------ 48 | * pick_ik inverse kinematics plugin compatible with MoveIt 2 49 | * Numeric gradient descent (local) solver 50 | * Memetic algorithm (global solver), configurable for single or multi-threading 51 | * Basic goal functions: joint centering, avoid joint limits, minimal joint displacement 52 | * Support for position-only IK and approximate solutions 53 | * Dynamic parameter switching at runtime using `generate_parameter_library `_ 54 | * Docker devcontainer workflow for VSCode 55 | * Contributors: Chris Thrasher, Sebastian Castro, Tyler Weaver 56 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(pick_ik) 3 | 4 | if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") 5 | add_compile_options(-Wall -Wextra -Wpedantic -Wshadow -Wconversion -Wsign-conversion -Wold-style-cast) 6 | endif() 7 | 8 | find_package(ament_cmake REQUIRED) 9 | find_package(ament_cmake_ros REQUIRED) 10 | 11 | find_package(fmt REQUIRED) 12 | find_package(generate_parameter_library REQUIRED) 13 | find_package(moveit_core REQUIRED) 14 | find_package(pluginlib REQUIRED) 15 | find_package(range-v3 REQUIRED) 16 | find_package(rclcpp REQUIRED) 17 | find_package(rsl REQUIRED) 18 | find_package(tf2_geometry_msgs REQUIRED) 19 | find_package(tf2_kdl REQUIRED) 20 | find_package(tl_expected REQUIRED) 21 | 22 | generate_parameter_library( 23 | pick_ik_parameters 24 | src/pick_ik_parameters.yaml 25 | ) 26 | add_library(pick_ik_plugin SHARED 27 | src/fk_moveit.cpp 28 | src/forward_kinematics.cpp 29 | src/pick_ik_plugin.cpp 30 | src/goal.cpp 31 | src/ik_memetic.cpp 32 | src/ik_gradient.cpp 33 | src/robot.cpp 34 | ) 35 | target_compile_features(pick_ik_plugin PUBLIC c_std_99 cxx_std_17) 36 | target_include_directories(pick_ik_plugin PUBLIC 37 | $ 38 | $) 39 | target_link_libraries(pick_ik_plugin 40 | PUBLIC 41 | moveit_core::moveit_robot_model 42 | tf2_geometry_msgs::tf2_geometry_msgs 43 | tf2_kdl::tf2_kdl 44 | tl_expected::tl_expected 45 | rsl::rsl 46 | PRIVATE 47 | fmt::fmt 48 | pick_ik_parameters 49 | moveit_core::moveit_kinematics_base 50 | moveit_core::moveit_robot_state 51 | pluginlib::pluginlib 52 | range-v3::range-v3 53 | rclcpp::rclcpp 54 | ) 55 | 56 | pluginlib_export_plugin_description_file( 57 | moveit_core 58 | pick_ik_kinematics_description.xml 59 | ) 60 | 61 | if(BUILD_TESTING) 62 | add_subdirectory(tests) 63 | endif() 64 | 65 | install( 66 | DIRECTORY include/ 67 | DESTINATION include/pick_ik 68 | ) 69 | install( 70 | TARGETS pick_ik_plugin pick_ik_parameters 71 | EXPORT export_pick_ik 72 | ARCHIVE DESTINATION lib 73 | LIBRARY DESTINATION lib 74 | RUNTIME DESTINATION bin 75 | ) 76 | 77 | ament_export_targets(export_pick_ik HAS_LIBRARY_TARGET) 78 | ament_export_dependencies( 79 | fmt 80 | moveit_core 81 | pluginlib 82 | range-v3 83 | rclcpp 84 | rsl 85 | tf2_geometry_msgs 86 | tf2_kdl 87 | tl_expected 88 | ) 89 | ament_package() 90 | -------------------------------------------------------------------------------- /CMakePresets.json: -------------------------------------------------------------------------------- 1 | { 2 | "version": 3, 3 | "cmakeMinimumRequired": { 4 | "major": 3, 5 | "minor": 22, 6 | "patch": 0 7 | }, 8 | "configurePresets": [ 9 | { 10 | "name": "relwithdebinfo", 11 | "displayName": "RelWithDebInfo", 12 | "generator": "Ninja", 13 | "binaryDir": "${sourceDir}/build/relwithdebinfo", 14 | "cacheVariables": { 15 | "CMAKE_BUILD_TYPE": "RelWithDebInfo", 16 | "CMAKE_C_COMPILER_LAUNCHER": "ccache", 17 | "CMAKE_CXX_COMPILER_LAUNCHER": "ccache", 18 | "CMAKE_EXPORT_COMPILE_COMMANDS": "ON", 19 | "CMAKE_EXE_LINKER_FLAGS": "-fuse-ld=lld", 20 | "CMAKE_MODULE_LINKER_FLAGS": "-fuse-ld=lld", 21 | "CMAKE_SHARED_LINKER_FLAGS": "-fuse-ld=lld", 22 | "CMAKE_CXX_FLAGS": "-Werror" 23 | }, 24 | "warnings": { 25 | "unusedCli": false 26 | } 27 | }, 28 | { 29 | "name": "debug", 30 | "inherits": "relwithdebinfo", 31 | "displayName": "Debug", 32 | "binaryDir": "${sourceDir}/build/debug", 33 | "cacheVariables": { 34 | "CMAKE_BUILD_TYPE": "Debug" 35 | } 36 | }, 37 | { 38 | "name": "release", 39 | "inherits": "relwithdebinfo", 40 | "displayName": "Release", 41 | "binaryDir": "${sourceDir}/build/release", 42 | "cacheVariables": { 43 | "CMAKE_BUILD_TYPE": "Release" 44 | } 45 | }, 46 | { 47 | "name": "asan", 48 | "inherits": "debug", 49 | "displayName": "Address sanitizer debug", 50 | "binaryDir": "${sourceDir}/build/asan", 51 | "cacheVariables": { 52 | "CMAKE_C_FLAGS": "-fsanitize=address", 53 | "CMAKE_CXX_FLAGS": "-fsanitize=address" 54 | } 55 | } 56 | ], 57 | "buildPresets": [ 58 | { 59 | "name": "relwithdebinfo", 60 | "configurePreset": "relwithdebinfo" 61 | }, 62 | { 63 | "name": "debug", 64 | "configurePreset": "debug" 65 | }, 66 | { 67 | "name": "release", 68 | "configurePreset": "release" 69 | }, 70 | { 71 | "name": "asan", 72 | "configurePreset": "asan" 73 | } 74 | ], 75 | "testPresets": [ 76 | { 77 | "name": "relwithdebinfo", 78 | "configurePreset": "relwithdebinfo", 79 | "output": { 80 | "outputOnFailure": true 81 | }, 82 | "execution": { 83 | "noTestsAction": "error", 84 | "stopOnFailure": true 85 | } 86 | } 87 | ] 88 | } 89 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Redistribution and use in source and binary forms, with or without 2 | modification, are permitted provided that the following conditions are met: 3 | 4 | * Redistributions of source code must retain the above copyright 5 | notice, this list of conditions and the following disclaimer. 6 | 7 | * Redistributions in binary form must reproduce the above copyright 8 | notice, this list of conditions and the following disclaimer in the 9 | documentation and/or other materials provided with the distribution. 10 | 11 | * Neither the name of the copyright holder nor the names of its 12 | contributors may be used to endorse or promote products derived from 13 | this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 19 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # pick_ik 2 | 3 | `pick_ik` is an inverse kinematics (IK) solver compatible with [MoveIt 2](https://github.com/ros-planning/moveit2). 4 | 5 | The solver is a reimplementation of [`bio_ik`](https://github.com/TAMS-Group/bio_ik), which combines: 6 | * A local optimizer which solves inverse kinematics via gradient descent 7 | * A global optimizer based on evolutionary algorithms 8 | 9 | Critically, `pick_ik` allows you to specify custom cost functions as discussed in [this paper](https://ieeexplore.ieee.org/document/8460799), so you can prioritize additional objectives than simply solving inverse kinematics at a specific frame. For example, you can minimize joint displacement from the initial guess, enforce that joints are close to a particular pose, or even pass custom cost functions to the plugin. 10 | 11 | If you are familiar with `bio_ik`, the functionality in this package includes: 12 | * Reimplementation of the memetic solver (equivalent to `bio1` and `bio2_memetic` solvers) 13 | * Reimplementation of the numeric gradient descent solvers (equivalent to `gd`, `gd_r`, and `gd_c` solvers) 14 | * Fully configurable number of threads if using the global solver 15 | * Cost functions on joint displacement, joint centering, and avoiding joint limits 16 | 17 | For more details on the implementation, take a look at the [paper](https://ieeexplore.ieee.org/document/8449979) or the [full thesis](https://d-nb.info/1221720910/34). 18 | 19 | --- 20 | 21 | ## Getting Started 22 | 23 | To get started using `pick_ik`, refer to the following README files: 24 | 25 | * [Installation](doc/INSTALL.md) 26 | * [Usage](doc/USAGE.md) 27 | * [MoveIt Tutorial](https://moveit.picknik.ai/main/doc/how_to_guides/pick_ik/pick_ik_tutorial.html) 28 | -------------------------------------------------------------------------------- /cmake/FindCatch2.cmake: -------------------------------------------------------------------------------- 1 | include(FetchContent) 2 | 3 | FetchContent_Declare(Catch2 4 | GIT_REPOSITORY https://github.com/catchorg/Catch2.git 5 | GIT_TAG v${Catch2_FIND_VERSION}) 6 | FetchContent_MakeAvailable(Catch2) 7 | set_target_properties(Catch2 PROPERTIES COMPILE_OPTIONS "" EXPORT_COMPILE_COMMANDS OFF) 8 | set_target_properties(Catch2WithMain PROPERTIES EXPORT_COMPILE_COMMANDS OFF) 9 | get_target_property(CATCH2_INCLUDE_DIRS Catch2 INTERFACE_INCLUDE_DIRECTORIES) 10 | target_include_directories(Catch2 SYSTEM INTERFACE ${CATCH2_INCLUDE_DIRS}) 11 | include(Catch) 12 | -------------------------------------------------------------------------------- /doc/INSTALL.md: -------------------------------------------------------------------------------- 1 | # pick_ik : Installation 2 | 3 | ## Install from binaries 4 | 5 | You can install the latest release of `pick_ik` from binaries using: 6 | 7 | ``` 8 | sudo apt install ros-${ROS_DISTRO}-pick-ik 9 | ``` 10 | 11 | where `${ROS_DISTRO}` refers to your ROS distribution (`humble`, `rolling`, etc.). 12 | 13 | --- 14 | 15 | ## Install from source 16 | 17 | 1. Create a colcon workspace: 18 | 19 | ```shell 20 | export COLCON_WS=~/ws_moveit2/ 21 | mkdir -p $COLCON_WS/src 22 | ``` 23 | 24 | 2. Clone this repository in the `src` directory of your workspace. 25 | 26 | ```shell 27 | cd $COLCON_WS/src 28 | git clone -b main https://github.com/PickNikRobotics/pick_ik.git 29 | ``` 30 | 31 | 3. Set up colcon mixins. 32 | 33 | ```shell 34 | sudo apt install python3-colcon-common-extensions 35 | sudo apt install python3-colcon-mixin 36 | colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml 37 | colcon mixin update default 38 | ``` 39 | 40 | 4. Build the workspace. 41 | 42 | ```shell 43 | cd /path/to/your/workspace 44 | colcon build --mixin release 45 | ``` 46 | 47 | --- 48 | 49 | ## Local development in Dev Containers 50 | 51 | This repo is also set up for VSCode Dev Containers, so you can develop directly in a Docker container. 52 | 53 | 1. Install Docker and add yourself to the Docker group. 54 | 55 | ```shell 56 | curl -fsSL https://get.docker.com -o get-docker.sh 57 | sudo sh get-docker.sh 58 | 59 | sudo groupadd docker 60 | sudo usermod -aG docker $USER 61 | newgrp docker 62 | ``` 63 | 64 | 2. Run these commands to create a directory to mount for ccache and another to mount for the ros directory containing log files. 65 | 66 | ```bash 67 | mkdir -p ~/.local/.pick_ik/ccache 68 | mkdir -p ~/.local/.pick_ik/ros 69 | ``` 70 | 71 | 3. Open the project in VSCode and follow the prompts to open the project in a Dev Container. 72 | -------------------------------------------------------------------------------- /doc/USAGE.md: -------------------------------------------------------------------------------- 1 | # pick_ik : Usage 2 | 3 | ## Using pick_ik as a Kinematics Plugin 4 | 5 | As discussed in the MoveIt 2 documentation, you can use the [MoveIt Setup Assistant](https://moveit.picknik.ai/humble/doc/examples/setup_assistant/setup_assistant_tutorial.html) or change the [`kinematics.yaml` file](https://moveit.picknik.ai/main/doc/examples/kinematics_configuration/kinematics_configuration_tutorial.html?highlight=kinematics%20yaml#kinematics-configuration) for your robot setup to use `pick_ik` as the IK solver. 6 | 7 | An example `kinematics.yaml` file might look as follows: 8 | 9 | ```yaml 10 | panda_arm: 11 | kinematics_solver: pick_ik/PickIkPlugin 12 | kinematics_solver_timeout: 0.05 13 | kinematics_solver_attempts: 3 14 | mode: global 15 | stop_optimization_on_valid_solution: true 16 | position_scale: 1.0 17 | rotation_scale: 0.5 18 | position_threshold: 0.001 19 | orientation_threshold: 0.01 20 | cost_threshold: 0.001 21 | minimal_displacement_weight: 0.0 22 | gd_step_size: 0.0001 23 | ``` 24 | 25 | As a sanity check, you could follow the [MoveIt Quickstart in RViz](https://moveit.picknik.ai/humble/doc/tutorials/quickstart_in_rviz/quickstart_in_rviz_tutorial.html) tutorial and change the `moveit_resources/panda_moveit_config/config/kinematics.yaml` file to use a configuration like the one above. 26 | 27 | --- 28 | 29 | ## Parameter Description 30 | 31 | For an exhaustive list of parameters, refer to the [parameters YAML file](../src/pick_ik_parameters.yaml). 32 | 33 | Some key parameters you may want to start with are: 34 | 35 | * `mode`: If you choose `local`, this solver will only do local gradient descent; if you choose `global`, it will also enable the evolutionary algorithm. Using the global solver will be less performant, but if you're having trouble getting out of local minima, this could help you. We recommend using `local` for things like relative motion / Cartesian interpolation / endpoint jogging, and `global` if you need to solve for goals with a far-away initial conditions. 36 | * `stop_optimization_on_valid_solution`: The default mode of pick_ik is to give you the first valid solution (which satisfies all thresholds) to make IK calls quick. Set this parameter to true if you rather want to use your complete computational budget (based on `kinematics_solver_timeout` and the maximum number of iterations of the solvers) to try to find a solution with a low cost value. 37 | * `memetic_`: All the properties that only kick in if you use the `global` solver. The key one is `memetic_num_threads`, as we have enabled the evolutionary algorithm to solve on multiple threads. 38 | * `cost_threshold`: This solver works by setting up cost functions based on how far away your pose is, how much your joints move relative to the initial guess, and custom cost functions you can add. Optimization succeeds only if the cost is less than `cost_threshold`. Note that if you're adding custom cost functions, you may want to set this threshold fairly high and rely on `position_threshold` and `orientation_threshold` to be your deciding factors, whereas this is more of a guideline. 39 | * `position_threshold`/`orientation_threshold`: Optimization succeeds only if the pose difference is less than these thresholds in meters and radians respectively. A `position_threshold` of 0.001 would mean a 1 mm accuracy and an `orientation_threshold` of 0.01 would mean a 0.01 radian accuracy. 40 | * `approximate_solution_position_threshold`/`approximate_solution_orientation_threshold`: When using approximate IK solutions for applications such as endpoint servoing, `pick_ik` may sometimes return solutions that are significantly far from the goal frame. To prevent issues with such jumps in solutions, these parameters define maximum translational and rotation displacement. We recommend setting this to values around a few centimeters and a few degrees for most applications. 41 | * `position_scale`: If you want rotation-only IK, set this to 0.0. If you want to solve for a custom `IKCostFn` (which you provide in your `setFromIK()` call) set this and `rotation_scale` to 0.0. You can also use any value other value to weight the position goal; it's part of the cost function. Note that any checks using `position_threshold` will be ignored if you use `position_scale = 0.0`. 42 | * `rotation_scale`: If you want position-only IK, set this to 0.0. If you want to treat position and orientation equally, set this to 1.0. You can also use any value in between; it's part of the cost function. Note that any checks using `orientation_threshold` will be ignored if you use `rotation_scale = 0.0`. 43 | * `minimal_displacement_weight`: This is one of the standard cost functions that checks for the joint angle difference between the initial guess and the solution. If you're solving for far-away goals, leave it to zero or it will hike up your cost function for no reason. Have this to a small non-zero value (e.g., 0.001) if you're doing things like Cartesian interpolation along a path, or endpoint jogging for servoing. 44 | 45 | You can test out this solver live in RViz, as this plugin uses the [`generate_parameter_library`](https://github.com/PickNikRobotics/generate_parameter_library) package to respond to parameter changes at every solve. This means that you can change values on the fly using the ROS 2 command-line interface, e.g., 46 | 47 | ```shell 48 | ros2 param set /rviz2 robot_description_kinematics.panda_arm.mode global 49 | 50 | ros2 param set /rviz2 robot_description_kinematics.panda_arm.minimal_displacement_weight 0.001 51 | ``` 52 | 53 | --- 54 | 55 | ## Custom Cost Functions 56 | 57 | The [kinematics plugin](../src/pick_ik_plugin.cpp) allows you to pass in an additional argument of type `IkCostFn`, which can be passed in from common entrypoints such as `RobotState::setFromIK()`. See [this page](https://moveit.picknik.ai/humble/doc/examples/robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html?highlight=setfromik#inverse-kinematics) for a usage example. 58 | Keep in mind that you need to set `position_scale = 0.0` and `rotation_scale = 0.0` if you want to calculate the costs solely based on your `IkCostFn`. 59 | If these parameters are non-zero, the target pose will still be part of the overall cost function. 60 | Additionally, you might want to define values for the `cost_threshold`, `approximate_solution_cost_threshold`, and `stop_optimization_on_valid_solution` parameters to decide when pick_ik will stop optimizing for your cost function and which solutions it should accept. 61 | 62 | Alternatively, consider adding your own cost functions to the `pick_ik` source code (specifically, in [`goal.hpp`](../include/goal.hpp) and [`goal.cpp`](../src/goal.cpp)) and submit a pull request with the new functionality you add. 63 | -------------------------------------------------------------------------------- /include/pick_ik/fk_moveit.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace pick_ik { 12 | 13 | using FkFn = std::function(std::vector const&)>; 14 | 15 | auto make_fk_fn(std::shared_ptr robot_model, 16 | moveit::core::JointModelGroup const* jmg, 17 | std::mutex& mx, 18 | std::vector tip_link_indices) -> FkFn; 19 | 20 | } // namespace pick_ik 21 | -------------------------------------------------------------------------------- /include/pick_ik/forward_kinematics.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace pick_ik { 10 | 11 | auto make_joint_axes(std::shared_ptr const& model) 12 | -> std::vector; 13 | 14 | auto make_link_frames(std::shared_ptr const& model) 15 | -> std::vector; 16 | 17 | auto get_frame(moveit::core::JointModel const& joint_model, 18 | std::vector const& variables, 19 | std::vector const& joint_axes) -> Eigen::Isometry3d; 20 | 21 | auto get_frame(moveit::core::LinkModel const& link_model, 22 | std::vector const& link_frame) -> Eigen::Isometry3d; 23 | 24 | auto has_joint_moved(moveit::core::JointModel const& joint_model, 25 | std::vector const& cached_variables, 26 | std::vector const& variables) -> bool; 27 | 28 | struct CachedJointFrames { 29 | std::vector variables; 30 | std::vector frames; 31 | }; 32 | 33 | auto get_frame(CachedJointFrames& cache, 34 | moveit::core::JointModel const& joint_model, 35 | std::vector const& variables, 36 | std::vector const& joint_axes) -> Eigen::Isometry3d; 37 | 38 | } // namespace pick_ik 39 | -------------------------------------------------------------------------------- /include/pick_ik/goal.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace pick_ik { 15 | 16 | // Frame equality tests 17 | using FrameTestFn = std::function; 18 | auto make_frame_tests(std::vector goal_frames, 19 | std::optional position_threshold = std::nullopt, 20 | std::optional orientation_threshold = std::nullopt) 21 | -> std::vector; 22 | 23 | // Pose cost functions 24 | using PoseCostFn = std::function const& tip_frames)>; 25 | auto make_pose_cost_fn(Eigen::Isometry3d goal, 26 | size_t goal_link_index, 27 | double position_scale, 28 | double rotation_scale) -> PoseCostFn; 29 | 30 | auto make_pose_cost_functions(std::vector goal_frames, 31 | double position_scale, 32 | double rotation_scale) -> std::vector; 33 | 34 | // Goal Function type 35 | using CostFn = std::function const& active_positions)>; 36 | 37 | struct Goal { 38 | CostFn eval; 39 | double weight; 40 | }; 41 | 42 | auto make_center_joints_cost_fn(Robot robot) -> CostFn; 43 | 44 | auto make_avoid_joint_limits_cost_fn(Robot robot) -> CostFn; 45 | 46 | auto make_minimal_displacement_cost_fn(Robot robot, std::vector initial_guess) -> CostFn; 47 | 48 | auto make_ik_cost_fn(geometry_msgs::msg::Pose pose, 49 | kinematics::KinematicsBase::IKCostFn cost_fn, 50 | std::shared_ptr robot_model, 51 | moveit::core::JointModelGroup const* jmg, 52 | std::vector initial_guess) -> CostFn; 53 | 54 | // Create a solution test function from frame tests and goals 55 | using SolutionTestFn = std::function const& active_positions)>; 56 | 57 | auto make_is_solution_test_fn(std::vector frame_tests, 58 | std::vector goals, 59 | double cost_threshold, 60 | FkFn fk) -> SolutionTestFn; 61 | 62 | using CostFn = std::function const& active_positions)>; 63 | 64 | auto make_cost_fn(std::vector pose_cost_functions, std::vector goals, FkFn fk) 65 | -> CostFn; 66 | 67 | } // namespace pick_ik 68 | -------------------------------------------------------------------------------- /include/pick_ik/ik_gradient.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace pick_ik { 14 | 15 | struct GradientIkParams { 16 | double step_size = 0.0001; // Step size for gradient descent. 17 | double min_cost_delta = 1.0e-12; // Minimum cost difference for termination. 18 | double max_time = 0.05; // Maximum time elapsed for termination. 19 | int max_iterations = 100; // Maximum iterations for termination. 20 | // If false, keeps running after finding a solution to further optimize the solution until a 21 | // time or iteration limit is reached. If true, stop thread on finding a valid solution. 22 | bool stop_optimization_on_valid_solution = true; 23 | }; 24 | 25 | struct GradientIk { 26 | std::vector gradient; 27 | std::vector working; 28 | std::vector local; 29 | std::vector best; 30 | double local_cost; 31 | double best_cost; 32 | 33 | static GradientIk from(std::vector const& initial_guess, CostFn const& cost_fn); 34 | }; 35 | 36 | /// Performs one step of gradient descent. 37 | /// @param self Instance of GradientIk object. 38 | /// @param robot Robot model, 39 | /// @param cost_fn Cost function for gradient descent. 40 | /// @param step_size Numerical step size for gradient descent. 41 | /// @return true if the cost function improved (decreased), else false. 42 | auto step(GradientIk& self, Robot const& robot, CostFn const& cost_fn, double step_size) -> bool; 43 | 44 | auto ik_gradient(std::vector const& initial_guess, 45 | Robot const& robot, 46 | CostFn const& cost_fn, 47 | SolutionTestFn const& solution_fn, 48 | GradientIkParams const& params, 49 | bool approx_solution) -> std::optional>; 50 | 51 | } // namespace pick_ik 52 | -------------------------------------------------------------------------------- /include/pick_ik/ik_memetic.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace pick_ik { 18 | 19 | struct Individual { 20 | std::vector genes; // Joint angles 21 | double fitness; 22 | double extinction; 23 | std::vector gradient; 24 | }; 25 | 26 | struct MemeticIkParams { 27 | // Evolutionary algorithm parameters 28 | size_t elite_size = 4; // Number of "keep alive" population members. 29 | size_t population_size = 16; // Number of total population members. 30 | double wipeout_fitness_tol = 0.00001; // Min fitness must improve by at least this much or the 31 | // population is reinitialized. 32 | int max_generations = 100; // Maximum iterations for evolutionary algorithm. 33 | double max_time = 1.0; // Maximum time for evolutionary algorithm. 34 | 35 | size_t num_threads = 1; // Number of species to solve in parallel. 36 | // If false, keeps running after finding a solution to further optimize the solution until a 37 | // time or iteration limit is reached. If true, stop thread on finding a valid solution. 38 | bool stop_optimization_on_valid_solution = true; 39 | // If true, returns first solution and terminates other threads. 40 | // If false, waits for all threads to join and returns best solution. 41 | bool stop_on_first_soln = true; 42 | 43 | // Gradient descent parameters for memetic exploitation. 44 | GradientIkParams gd_params; 45 | }; 46 | 47 | class MemeticIk { 48 | private: 49 | // Evolutionary algorithm values 50 | std::vector population_; 51 | std::vector mating_pool_; 52 | Individual best_; // Best solution overall. 53 | Individual best_curr_; // Best solution so far. 54 | std::optional previous_fitness_; 55 | 56 | // Solver parameters 57 | MemeticIkParams params_; 58 | 59 | // Scaling coefficients (cached since they do not change). 60 | std::vector extinction_grading_; 61 | double inverse_gene_size_; 62 | 63 | public: 64 | MemeticIk(std::vector const& initial_guess, double cost, MemeticIkParams const& params); 65 | static MemeticIk from(std::vector const& initial_guess, 66 | CostFn const& cost_fn, 67 | MemeticIkParams const& params); 68 | 69 | Individual best() const { return best_; }; 70 | Individual bestCurrent() const { return best_curr_; }; 71 | size_t eliteCount() const { return params_.elite_size; }; 72 | bool checkWipeout(); 73 | void computeExtinctions(); 74 | void gradientDescent(size_t const i, 75 | Robot const& robot, 76 | CostFn const& cost_fn, 77 | GradientIkParams const& gd_params); 78 | void initPopulation(Robot const& robot, 79 | CostFn const& cost_fn, 80 | std::vector const& initial_guess); 81 | void reproduce(Robot const& robot, CostFn const& cost_fn); 82 | size_t populationCount() const { return params_.population_size; }; 83 | void printPopulation() const; 84 | void sortPopulation(); 85 | }; 86 | 87 | // Implementation of memetic IK solve. 88 | auto ik_memetic_impl(std::vector const& initial_guess, 89 | Robot const& robot, 90 | CostFn const& cost_fn, 91 | SolutionTestFn const& solution_fn, 92 | MemeticIkParams const& params, 93 | std::atomic& terminate, 94 | bool approx_solution = false, 95 | bool print_debug = false) -> std::optional; 96 | 97 | // Top-level IK solution implementation that handles single vs. multithreading. 98 | auto ik_memetic(std::vector const& initial_guess, 99 | Robot const& robot, 100 | CostFn const& cost_fn, 101 | SolutionTestFn const& solution_fn, 102 | MemeticIkParams const& params, 103 | bool approx_solution = false, 104 | bool print_debug = false) -> std::optional>; 105 | 106 | } // namespace pick_ik 107 | -------------------------------------------------------------------------------- /include/pick_ik/pick_ik_plugin.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace pick_ik { 8 | 9 | class ParamListener; 10 | 11 | class PickIKPlugin : public kinematics::KinematicsBase { 12 | rclcpp::Node::SharedPtr node_; 13 | std::shared_ptr parameter_listener_; 14 | moveit::core::JointModelGroup const* jmg_; 15 | 16 | std::vector joint_names_; 17 | std::vector link_names_; 18 | std::vector tip_link_indices_; 19 | Robot robot_; 20 | 21 | mutable std::mutex fk_mutex_; 22 | 23 | public: 24 | virtual bool initialize(rclcpp::Node::SharedPtr const& node, 25 | moveit::core::RobotModel const& robot_model, 26 | std::string const& group_name, 27 | std::string const& base_frame, 28 | std::vector const& tip_frames, 29 | double search_discretization); 30 | 31 | virtual bool searchPositionIK( 32 | std::vector const& ik_poses, 33 | std::vector const& ik_seed_state, 34 | double timeout, 35 | std::vector const&, 36 | std::vector& solution, 37 | IKCallbackFn const& solution_callback, 38 | IKCostFn const& cost_function, 39 | moveit_msgs::msg::MoveItErrorCodes& error_code, 40 | kinematics::KinematicsQueryOptions const& options = kinematics::KinematicsQueryOptions(), 41 | moveit::core::RobotState const* context_state = nullptr) const; 42 | 43 | virtual std::vector const& getJointNames() const; 44 | 45 | virtual std::vector const& getLinkNames() const; 46 | 47 | virtual bool getPositionFK(std::vector const&, 48 | std::vector const&, 49 | std::vector&) const; 50 | 51 | virtual bool getPositionIK(geometry_msgs::msg::Pose const&, 52 | std::vector const&, 53 | std::vector&, 54 | moveit_msgs::msg::MoveItErrorCodes&, 55 | kinematics::KinematicsQueryOptions const&) const; 56 | 57 | virtual bool searchPositionIK(geometry_msgs::msg::Pose const& ik_pose, 58 | std::vector const& ik_seed_state, 59 | double timeout, 60 | std::vector& solution, 61 | moveit_msgs::msg::MoveItErrorCodes& error_code, 62 | kinematics::KinematicsQueryOptions const& options = 63 | kinematics::KinematicsQueryOptions()) const; 64 | 65 | virtual bool searchPositionIK(geometry_msgs::msg::Pose const& ik_pose, 66 | std::vector const& ik_seed_state, 67 | double timeout, 68 | std::vector const& consistency_limits, 69 | std::vector& solution, 70 | moveit_msgs::msg::MoveItErrorCodes& error_code, 71 | kinematics::KinematicsQueryOptions const& options = 72 | kinematics::KinematicsQueryOptions()) const; 73 | 74 | virtual bool searchPositionIK(geometry_msgs::msg::Pose const& ik_pose, 75 | std::vector const& ik_seed_state, 76 | double timeout, 77 | std::vector& solution, 78 | IKCallbackFn const& solution_callback, 79 | moveit_msgs::msg::MoveItErrorCodes& error_code, 80 | kinematics::KinematicsQueryOptions const& options = 81 | kinematics::KinematicsQueryOptions()) const; 82 | 83 | virtual bool searchPositionIK(geometry_msgs::msg::Pose const& ik_pose, 84 | std::vector const& ik_seed_state, 85 | double timeout, 86 | std::vector const& consistency_limits, 87 | std::vector& solution, 88 | IKCallbackFn const& solution_callback, 89 | moveit_msgs::msg::MoveItErrorCodes& error_code, 90 | kinematics::KinematicsQueryOptions const& options = 91 | kinematics::KinematicsQueryOptions()) const; 92 | 93 | virtual bool searchPositionIK( 94 | std::vector const& ik_poses, 95 | std::vector const& ik_seed_state, 96 | double timeout, 97 | std::vector const& consistency_limits, 98 | std::vector& solution, 99 | IKCallbackFn const& solution_callback, 100 | moveit_msgs::msg::MoveItErrorCodes& error_code, 101 | kinematics::KinematicsQueryOptions const& options = kinematics::KinematicsQueryOptions(), 102 | moveit::core::RobotState const* context_state = NULL) const; 103 | }; 104 | 105 | } // namespace pick_ik 106 | -------------------------------------------------------------------------------- /include/pick_ik/robot.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace pick_ik { 13 | 14 | struct Robot { 15 | struct Variable { 16 | /// @brief Min, max, and middle position values of the variable. 17 | double min, max, mid; 18 | 19 | /// @brief Whether the variable's position is bounded. 20 | bool bounded; 21 | 22 | /// @brief The half-span (min - max) / 2.0 of the variable, or a default value if unbounded. 23 | double half_span; 24 | 25 | double max_velocity_rcp; 26 | double minimal_displacement_factor; 27 | 28 | /// @brief Generates a valid variable value given an optional initial value (for unbounded 29 | /// joints). 30 | auto generate_valid_value(double init_val = 0.0) const -> double; 31 | 32 | /// @brief Returns true if a value is valid given the variable bounds. 33 | auto is_valid(double val) const -> bool; 34 | 35 | /// @brief Clamps a configuration to joint limits. 36 | auto clamp_to_limits(double val) const -> double; 37 | }; 38 | std::vector variables; 39 | 40 | /** @brief Create new Robot from a RobotModel. */ 41 | static auto from(std::shared_ptr const& model, 42 | moveit::core::JointModelGroup const* jmg, 43 | std::vector tip_link_indices) -> Robot; 44 | 45 | /** 46 | * @brief Sets a variable vector to a random configuration. 47 | * @details Here, "valid" denotes that the joint values are with their specified limits. 48 | */ 49 | auto set_random_valid_configuration(std::vector& config) const -> void; 50 | 51 | /** @brief Check is a configuration is valid. */ 52 | auto is_valid_configuration(std::vector const& config) const -> bool; 53 | }; 54 | 55 | auto get_link_indices(std::shared_ptr const& model, 56 | std::vector const& names) 57 | -> tl::expected, std::string>; 58 | 59 | auto get_active_variable_indices(std::shared_ptr const& robot_model, 60 | moveit::core::JointModelGroup const* jmg, 61 | std::vector const& tip_link_indices) 62 | -> std::vector; 63 | 64 | auto get_minimal_displacement_factors(std::vector const& active_variable_indices, 65 | Robot const& robot) -> std::vector; 66 | 67 | auto get_variables(moveit::core::RobotState const& robot_state) -> std::vector; 68 | 69 | auto transform_poses_to_frames(moveit::core::RobotState const& robot_state, 70 | std::vector const& poses, 71 | std::string const& base_frame_name) 72 | -> std::vector; 73 | 74 | } // namespace pick_ik 75 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | pick_ik 5 | 1.1.1 6 | Inverse Kinematics solver for MoveIt 7 | Tyler Weaver 8 | Tyler Weaver 9 | BSD-3-Clause 10 | 11 | ament_cmake_ros 12 | 13 | fmt 14 | generate_parameter_library 15 | moveit_core 16 | pluginlib 17 | range-v3 18 | rclcpp 19 | rsl 20 | tf2_geometry_msgs 21 | tf2_kdl 22 | tl_expected 23 | 24 | moveit_resources_panda_moveit_config 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /pick_ik_kinematics_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /src/fk_moveit.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace pick_ik { 10 | 11 | auto make_fk_fn(std::shared_ptr robot_model, 12 | moveit::core::JointModelGroup const* jmg, 13 | std::mutex& mx, 14 | std::vector tip_link_indices) -> FkFn { 15 | auto robot_state = moveit::core::RobotState(robot_model); 16 | robot_state.setToDefaultValues(); 17 | 18 | // IK function is mutable so it re-uses the robot_state instead of creating 19 | // new copies. This function accepts a mutex so that it can be made thread-safe. 20 | return [=, &mx](std::vector const& active_positions) mutable { 21 | std::scoped_lock lock(mx); 22 | robot_state.setJointGroupPositions(jmg, active_positions); 23 | robot_state.updateLinkTransforms(); 24 | 25 | std::vector tip_frames; 26 | std::transform(tip_link_indices.cbegin(), 27 | tip_link_indices.cend(), 28 | std::back_inserter(tip_frames), 29 | [&](auto index) { 30 | auto const* link_model = robot_model->getLinkModel(index); 31 | return robot_state.getGlobalLinkTransform(link_model); 32 | }); 33 | return tip_frames; 34 | }; 35 | } 36 | 37 | } // namespace pick_ik 38 | -------------------------------------------------------------------------------- /src/forward_kinematics.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace pick_ik { 12 | 13 | auto make_joint_axes(std::shared_ptr const& model) 14 | -> std::vector { 15 | auto joint_axes = std::vector{}; 16 | joint_axes.resize(model->getJointModelCount()); 17 | for (size_t i = 0; i < joint_axes.size(); ++i) { 18 | auto const* joint_model = model->getJointModel(i); 19 | if (auto const* j = dynamic_cast(joint_model)) { 20 | joint_axes[i] = tf2::Vector3(j->getAxis().x(), j->getAxis().y(), j->getAxis().z()); 21 | } 22 | if (auto const* j = dynamic_cast(joint_model)) { 23 | joint_axes[i] = tf2::Vector3(j->getAxis().x(), j->getAxis().y(), j->getAxis().z()); 24 | } 25 | } 26 | return joint_axes; 27 | } 28 | 29 | auto make_link_frames(std::shared_ptr const& model) 30 | -> std::vector { 31 | std::vector link_frames; 32 | std::transform(model->getLinkModels().cbegin(), 33 | model->getLinkModels().cend(), 34 | link_frames.begin(), 35 | [](auto* link_model) { return link_model->getJointOriginTransform(); }); 36 | return link_frames; 37 | } 38 | 39 | auto get_frame(moveit::core::JointModel const& joint_model, 40 | std::vector const& variables, 41 | std::vector const& joint_axes) -> Eigen::Isometry3d { 42 | auto const type = joint_model.getType(); 43 | size_t const index = joint_model.getJointIndex(); 44 | 45 | switch (type) { 46 | case moveit::core::JointModel::FIXED: 47 | return Eigen::Isometry3d::Identity(); 48 | case moveit::core::JointModel::REVOLUTE: { 49 | auto const axis = joint_axes.at(index); 50 | auto const v = variables.at(joint_model.getFirstVariableIndex()); 51 | auto const half_angle = v * 0.5; 52 | auto const fcos = cos(half_angle); 53 | auto const fsin = sin(half_angle); 54 | 55 | return Eigen::Isometry3d( 56 | Eigen::Quaterniond(fcos, axis.x() * fsin, axis.y() * fsin, axis.z() * fsin)); 57 | } 58 | case moveit::core::JointModel::PRISMATIC: { 59 | auto const axis = joint_axes.at(index); 60 | auto const v = variables.at(joint_model.getFirstVariableIndex()); 61 | return Eigen::Isometry3d( 62 | Eigen::Translation3d(axis.x() * v, axis.y() * v, axis.z() * v)); 63 | } 64 | case moveit::core::JointModel::FLOATING: { 65 | assert(joint_model.getFirstVariableIndex() + 6 >= variables.size()); 66 | auto const* vv = variables.data() + joint_model.getFirstVariableIndex(); 67 | // Note that Eigen uses (w, x, y, z) for quaternion notation. 68 | return Eigen::Translation3d(vv[0], vv[1], vv[2]) * 69 | Eigen::Quaterniond(vv[6], vv[3], vv[4], vv[5]); 70 | } 71 | case moveit::core::JointModel::PLANAR: 72 | case moveit::core::JointModel::UNKNOWN: 73 | break; 74 | } 75 | 76 | auto const* joint_variables = variables.data() + joint_model.getFirstVariableIndex(); 77 | Eigen::Isometry3d joint_transform; 78 | joint_model.computeTransform(joint_variables, joint_transform); 79 | return joint_transform; 80 | } 81 | 82 | auto get_frame(moveit::core::LinkModel const& link_model, 83 | std::vector const& link_frames) -> Eigen::Isometry3d { 84 | return link_frames.at(link_model.getLinkIndex()); 85 | } 86 | 87 | auto has_joint_moved(moveit::core::JointModel const& joint_model, 88 | std::vector const& cached_variables, 89 | std::vector const& variables) -> bool { 90 | size_t const i0 = joint_model.getFirstVariableIndex(); 91 | size_t const cnt = joint_model.getVariableCount(); 92 | if (cnt == 0) return true; 93 | if (cnt == 1) return !(variables.at(i0) == cached_variables.at(i0)); 94 | for (size_t i = i0; i < i0 + cnt; ++i) { 95 | if (!(variables.at(i) == cached_variables.at(i))) { 96 | return true; 97 | } 98 | } 99 | return false; 100 | } 101 | 102 | auto get_frame(CachedJointFrames& cache, 103 | moveit::core::JointModel const& joint_model, 104 | std::vector const& variables, 105 | std::vector const& joint_axes) -> Eigen::Isometry3d { 106 | size_t const index = joint_model.getJointIndex(); 107 | 108 | if (!has_joint_moved(joint_model, cache.variables, variables)) { 109 | return cache.frames.at(index); 110 | } 111 | 112 | // Update frame in cache 113 | cache.frames.at(index) = get_frame(joint_model, variables, joint_axes); 114 | 115 | // Update variables in cache 116 | auto const cnt = joint_model.getVariableCount(); 117 | auto const i0 = joint_model.getFirstVariableIndex(); 118 | if (cnt > 0) { 119 | for (size_t i = i0; i < i0 + cnt; ++i) { 120 | cache.variables.at(i) = variables.at(i); 121 | } 122 | } 123 | 124 | return cache.frames.at(index); 125 | } 126 | 127 | } // namespace pick_ik 128 | -------------------------------------------------------------------------------- /src/goal.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace pick_ik { 16 | 17 | double linear_distance(Eigen::Isometry3d const& frame_1, Eigen::Isometry3d const& frame_2) { 18 | return (frame_1.translation() - frame_2.translation()).norm(); 19 | } 20 | 21 | double angular_distance(Eigen::Isometry3d const& frame_1, Eigen::Isometry3d const& frame_2) { 22 | auto const q_1 = Eigen::Quaterniond(frame_1.rotation()); 23 | auto const q_2 = Eigen::Quaterniond(frame_2.rotation()); 24 | return q_2.angularDistance(q_1); 25 | } 26 | 27 | auto make_frame_test_fn(Eigen::Isometry3d goal_frame, 28 | std::optional position_threshold = std::nullopt, 29 | std::optional orientation_threshold = std::nullopt) -> FrameTestFn { 30 | return [=](Eigen::Isometry3d const& tip_frame) -> bool { 31 | return (!position_threshold.has_value() || 32 | linear_distance(goal_frame, tip_frame) <= position_threshold) && 33 | (!orientation_threshold.has_value() || 34 | std::abs(angular_distance(goal_frame, tip_frame)) <= orientation_threshold.value()); 35 | }; 36 | } 37 | 38 | auto make_frame_tests(std::vector goal_frames, 39 | std::optional position_threshold, 40 | std::optional orientation_threshold) -> std::vector { 41 | auto tests = std::vector{}; 42 | std::transform(goal_frames.cbegin(), 43 | goal_frames.cend(), 44 | std::back_inserter(tests), 45 | [&](auto const& frame) { 46 | return make_frame_test_fn(frame, position_threshold, orientation_threshold); 47 | }); 48 | return tests; 49 | } 50 | 51 | auto make_pose_cost_fn(Eigen::Isometry3d goal, 52 | size_t goal_link_index, 53 | double position_scale, 54 | double rotation_scale) -> PoseCostFn { 55 | if (position_scale > 0.0) { 56 | if (rotation_scale > 0.0) { 57 | return [=](std::vector const& tip_frames) -> double { 58 | auto const& frame = tip_frames[goal_link_index]; 59 | return std::pow(linear_distance(goal, frame) * position_scale, 2) + 60 | std::pow(angular_distance(goal, frame) * rotation_scale, 2); 61 | }; 62 | } else { 63 | return [=](std::vector const& tip_frames) -> double { 64 | auto const& frame = tip_frames[goal_link_index]; 65 | return std::pow(linear_distance(goal, frame) * position_scale, 2); 66 | }; 67 | } 68 | } else { 69 | if (rotation_scale > 0.0) { 70 | return [=](std::vector const& tip_frames) -> double { 71 | auto const& frame = tip_frames[goal_link_index]; 72 | return std::pow(angular_distance(goal, frame) * rotation_scale, 2); 73 | }; 74 | } else { 75 | return [=](std::vector const&) -> double { return 0.0; }; 76 | } 77 | } 78 | } 79 | 80 | auto make_pose_cost_functions(std::vector goal_frames, 81 | double position_scale, 82 | double rotation_scale) -> std::vector { 83 | auto cost_functions = std::vector{}; 84 | for (size_t i = 0; i < goal_frames.size(); ++i) { 85 | cost_functions.push_back( 86 | make_pose_cost_fn(goal_frames[i], i, position_scale, rotation_scale)); 87 | } 88 | return cost_functions; 89 | } 90 | 91 | auto make_center_joints_cost_fn(Robot robot) -> CostFn { 92 | return [=](std::vector const& active_positions) -> double { 93 | double sum = 0; 94 | assert(robot.variables.size() == active_positions.size()); 95 | for (size_t i = 0; i < active_positions.size(); ++i) { 96 | auto const& variable = robot.variables[i]; 97 | if (!variable.bounded) { 98 | continue; 99 | } 100 | 101 | auto const position = active_positions[i]; 102 | auto const weight = variable.minimal_displacement_factor; 103 | auto const mid = (variable.min + variable.max) * 0.5; 104 | sum += std::pow((position - mid) * weight, 2); 105 | } 106 | return sum; 107 | }; 108 | } 109 | 110 | auto make_avoid_joint_limits_cost_fn(Robot robot) -> CostFn { 111 | return [=](std::vector const& active_positions) -> double { 112 | double sum = 0; 113 | assert(robot.variables.size() == active_positions.size()); 114 | for (size_t i = 0; i < active_positions.size(); ++i) { 115 | auto const& variable = robot.variables[i]; 116 | if (!variable.bounded) { 117 | continue; 118 | } 119 | 120 | auto const position = active_positions[i]; 121 | auto const weight = variable.minimal_displacement_factor; 122 | sum += std::pow( 123 | std::fmax(0.0, std::fabs(position - variable.mid) * 2.0 - variable.half_span) * 124 | weight, 125 | 2); 126 | } 127 | return sum; 128 | }; 129 | } 130 | 131 | auto make_minimal_displacement_cost_fn(Robot robot, std::vector initial_guess) -> CostFn { 132 | return [=](std::vector const& active_positions) -> double { 133 | double sum = 0; 134 | assert(active_positions.size() == robot.variables.size() && 135 | active_positions.size() == initial_guess.size()); 136 | for (size_t i = 0; i < active_positions.size(); ++i) { 137 | auto const guess = initial_guess[i]; 138 | auto const position = active_positions[i]; 139 | auto const weight = robot.variables[i].minimal_displacement_factor; 140 | sum += std::pow((position - guess) * weight, 2); 141 | } 142 | return sum; 143 | }; 144 | } 145 | 146 | auto make_ik_cost_fn(geometry_msgs::msg::Pose pose, 147 | kinematics::KinematicsBase::IKCostFn cost_fn, 148 | std::shared_ptr robot_model, 149 | moveit::core::JointModelGroup const* jmg, 150 | std::vector initial_guess) -> CostFn { 151 | auto robot_state = moveit::core::RobotState(robot_model); 152 | robot_state.setToDefaultValues(); 153 | robot_state.setJointGroupPositions(jmg, initial_guess); 154 | robot_state.update(); 155 | 156 | return [=](std::vector const& active_positions) mutable { 157 | robot_state.setJointGroupPositions(jmg, active_positions); 158 | robot_state.update(); 159 | return cost_fn(pose, robot_state, jmg, initial_guess); 160 | }; 161 | } 162 | 163 | auto make_is_solution_test_fn(std::vector frame_tests, 164 | std::vector goals, 165 | double cost_threshold, 166 | FkFn fk) -> SolutionTestFn { 167 | return [=](std::vector const& active_positions) { 168 | auto tip_frames = fk(active_positions); 169 | assert(frame_tests.size() == tip_frames.size()); 170 | for (size_t i = 0; i < frame_tests.size(); ++i) { 171 | if (!frame_tests[i](tip_frames[i])) { 172 | return false; 173 | } 174 | } 175 | 176 | auto const cost_threshold_sq = std::pow(cost_threshold, 2); 177 | for (auto const& goal : goals) { 178 | auto const cost = goal.eval(active_positions) * std::pow(goal.weight, 2); 179 | if (cost >= cost_threshold_sq) { 180 | return false; 181 | } 182 | } 183 | 184 | return true; 185 | }; 186 | } 187 | 188 | auto make_cost_fn(std::vector pose_cost_functions, std::vector goals, FkFn fk) 189 | -> CostFn { 190 | return [=](std::vector const& active_positions) { 191 | auto tip_frames = fk(active_positions); 192 | auto const pose_cost = 193 | std::accumulate(pose_cost_functions.cbegin(), 194 | pose_cost_functions.cend(), 195 | 0.0, 196 | [&](auto sum, auto const& fn) { return sum + fn(tip_frames); }); 197 | auto const goal_cost = 198 | std::accumulate(goals.cbegin(), goals.cend(), 0.0, [&](auto sum, auto const& goal) { 199 | return sum + goal.eval(active_positions) * std::pow(goal.weight, 2); 200 | }); 201 | return pose_cost + goal_cost; 202 | }; 203 | } 204 | 205 | } // namespace pick_ik 206 | -------------------------------------------------------------------------------- /src/ik_gradient.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace pick_ik { 13 | 14 | GradientIk GradientIk::from(std::vector const& initial_guess, CostFn const& cost_fn) { 15 | auto const initial_cost = cost_fn(initial_guess); 16 | return GradientIk{std::vector(initial_guess.size(), 0.0), 17 | initial_guess, 18 | initial_guess, 19 | initial_guess, 20 | initial_cost, 21 | initial_cost}; 22 | } 23 | 24 | auto step(GradientIk& self, Robot const& robot, CostFn const& cost_fn, double step_size) -> bool { 25 | auto const count = self.local.size(); 26 | 27 | // compute gradient direction 28 | for (size_t i = 0; i < count; ++i) { 29 | // test negative displacement 30 | self.working[i] = self.local[i] - step_size; 31 | double const p1 = cost_fn(self.working); 32 | 33 | // test positive displacement 34 | self.working[i] = self.local[i] + step_size; 35 | double const p3 = cost_fn(self.working); 36 | 37 | // reset self.working 38 | self.working[i] = self.local[i]; 39 | 40 | // + gradient = + on this joint increases cost fn result 41 | // - gradient = - on this joint increases cost fn result 42 | self.gradient[i] = p3 - p1; 43 | } 44 | 45 | // normalize gradient direction 46 | auto sum = std::accumulate(self.gradient.cbegin(), 47 | self.gradient.cend(), 48 | step_size, 49 | [](auto acc, auto value) { return acc + std::fabs(value); }); 50 | double const f = 1.0 / sum * step_size; 51 | std::transform(self.gradient.cbegin(), 52 | self.gradient.cend(), 53 | self.gradient.begin(), 54 | [&](auto value) { return value * f; }); 55 | 56 | // initialize line search 57 | for (size_t i = 0; i < count; ++i) { 58 | self.working[i] = self.local[i] - self.gradient[i]; 59 | } 60 | double const p1 = cost_fn(self.working); 61 | 62 | for (size_t i = 0; i < count; ++i) { 63 | self.working[i] = self.local[i] + self.gradient[i]; 64 | } 65 | double const p3 = cost_fn(self.working); 66 | double const p2 = (p1 + p3) * 0.5; 67 | 68 | // linear step size estimation 69 | double const cost_diff = (p3 - p1) * 0.5; 70 | double joint_diff = p2 / cost_diff; 71 | 72 | // if the cost_diff was 0 73 | if (!isfinite(joint_diff)) joint_diff = 0.0; 74 | 75 | // apply optimization step 76 | // (move along gradient direction by estimated step size) 77 | for (size_t i = 0; i < count; ++i) { 78 | auto const& var = robot.variables[i]; 79 | auto updated_value = self.local[i] - self.gradient[i] * joint_diff; 80 | self.working[i] = var.clamp_to_limits(updated_value); 81 | } 82 | 83 | // Always accept the solution and continue 84 | self.local = self.working; 85 | self.local_cost = cost_fn(self.local); 86 | 87 | // Update best solution 88 | if (self.local_cost < self.best_cost) { 89 | self.best = self.local; 90 | self.best_cost = self.local_cost; 91 | return true; 92 | } 93 | return false; 94 | } 95 | 96 | auto ik_gradient(std::vector const& initial_guess, 97 | Robot const& robot, 98 | CostFn const& cost_fn, 99 | SolutionTestFn const& solution_fn, 100 | GradientIkParams const& params, 101 | bool approx_solution) -> std::optional> { 102 | if (params.stop_optimization_on_valid_solution && solution_fn(initial_guess)) { 103 | return initial_guess; 104 | } 105 | 106 | assert(robot.variables.size() == initial_guess.size()); 107 | auto ik = GradientIk::from(initial_guess, cost_fn); 108 | 109 | // Main loop 110 | int num_iterations = 0; 111 | double previous_cost = 0.0; 112 | auto const timeout_point = 113 | std::chrono::system_clock::now() + std::chrono::duration(params.max_time); 114 | 115 | while ((std::chrono::system_clock::now() < timeout_point) && 116 | (num_iterations < params.max_iterations)) { 117 | if (step(ik, robot, cost_fn, params.step_size)) { 118 | if (params.stop_optimization_on_valid_solution && solution_fn(ik.best)) { 119 | return ik.best; 120 | } 121 | } 122 | 123 | if (abs(ik.local_cost - previous_cost) <= params.min_cost_delta) { 124 | break; 125 | } 126 | previous_cost = ik.local_cost; 127 | num_iterations++; 128 | } 129 | 130 | if (!params.stop_optimization_on_valid_solution && solution_fn(ik.best)) { 131 | return ik.best; 132 | } 133 | 134 | // If no solution was found, either return the approximate solution or nothing. 135 | if (approx_solution) { 136 | return ik.best; 137 | } 138 | return std::nullopt; 139 | } 140 | 141 | } // namespace pick_ik 142 | -------------------------------------------------------------------------------- /src/ik_memetic.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | namespace pick_ik { 17 | 18 | MemeticIk MemeticIk::from(std::vector const& initial_guess, 19 | CostFn const& cost_fn, 20 | MemeticIkParams const& params) { 21 | return MemeticIk{initial_guess, cost_fn(initial_guess), params}; 22 | } 23 | 24 | MemeticIk::MemeticIk(std::vector const& initial_guess, 25 | double cost, 26 | MemeticIkParams const& params) 27 | : params_{params} { 28 | best_ = Individual{initial_guess, cost, 0.0, std::vector(initial_guess.size(), 0.0)}; 29 | best_curr_ = best_; 30 | 31 | population_.reserve(params.population_size); 32 | mating_pool_.reserve(params.elite_size); 33 | 34 | // Cache some coefficients to not have to recompute them all the time. 35 | extinction_grading_.reserve(params.population_size); 36 | for (size_t i = 0; i < params.population_size; ++i) { 37 | extinction_grading_.push_back(static_cast(i) / 38 | static_cast(params.population_size - 1)); 39 | } 40 | inverse_gene_size_ = 1.0 / static_cast(initial_guess.size()); 41 | }; 42 | 43 | bool MemeticIk::checkWipeout() { 44 | // Handle wipeouts if no progress is being made. 45 | if (previous_fitness_.has_value()) { 46 | bool const improved = 47 | (best_curr_.fitness < previous_fitness_.value() - params_.wipeout_fitness_tol); 48 | if (!improved) { 49 | return true; 50 | } 51 | } 52 | 53 | previous_fitness_ = best_curr_.fitness; 54 | return false; 55 | } 56 | 57 | void MemeticIk::computeExtinctions() { 58 | double min_fitness = population_.front().fitness; 59 | double max_fitness = population_.back().fitness; 60 | for (size_t i = 0; i < params_.population_size; ++i) { 61 | population_[i].extinction = 62 | (population_[i].fitness + min_fitness * (extinction_grading_[i] - 1)) / max_fitness; 63 | } 64 | } 65 | 66 | void MemeticIk::gradientDescent(size_t const i, 67 | Robot const& robot, 68 | CostFn const& cost_fn, 69 | GradientIkParams const& gd_params) { 70 | auto& individual = population_[i]; 71 | auto local_ik = GradientIk::from(individual.genes, cost_fn); 72 | 73 | int num_iterations = 0; 74 | double previous_cost = 0; 75 | auto const timeout_point_local = 76 | std::chrono::system_clock::now() + std::chrono::duration(gd_params.max_time); 77 | 78 | while ((std::chrono::system_clock::now() < timeout_point_local) && 79 | (num_iterations < gd_params.max_iterations)) { 80 | step(local_ik, robot, cost_fn, gd_params.step_size); 81 | if (abs(local_ik.local_cost - previous_cost) <= gd_params.min_cost_delta) { 82 | break; 83 | } 84 | previous_cost = local_ik.local_cost; 85 | num_iterations++; 86 | } 87 | 88 | individual.genes = local_ik.best; 89 | individual.fitness = cost_fn(individual.genes); 90 | individual.gradient = local_ik.gradient; 91 | } 92 | 93 | void MemeticIk::initPopulation(Robot const& robot, 94 | CostFn const& cost_fn, 95 | std::vector const& initial_guess) { 96 | std::vector const zero_grad(robot.variables.size(), 0.0); 97 | population_.resize(params_.population_size); 98 | for (size_t i = 0; i < params_.elite_size; ++i) { 99 | auto genotype = initial_guess; 100 | if (i > 0) { 101 | robot.set_random_valid_configuration(genotype); 102 | } 103 | population_[i] = Individual{genotype, cost_fn(genotype), 1.0, zero_grad}; 104 | } 105 | 106 | // Initialize children to some dummy values that will be overwritten. 107 | for (size_t i = params_.elite_size; i < params_.population_size; ++i) { 108 | population_[i] = Individual{initial_guess, 0.0, 1.0, zero_grad}; 109 | } 110 | 111 | // Initialize fitnesses and extinctions 112 | for (auto& individual : population_) { 113 | individual.fitness = cost_fn(individual.genes); 114 | } 115 | computeExtinctions(); 116 | previous_fitness_.reset(); 117 | } 118 | 119 | void MemeticIk::reproduce(Robot const& robot, CostFn const& cost_fn) { 120 | // Reset mating pool 121 | mating_pool_.resize(params_.elite_size); 122 | for (size_t i = 0; i < params_.elite_size; ++i) { 123 | mating_pool_[i] = &population_[i]; 124 | } 125 | 126 | for (size_t i = params_.elite_size; i < params_.population_size; ++i) { 127 | // Select parents from the mating pool 128 | // Note that we permit there being only one parent, which basically counts as just 129 | // mutations. 130 | if (!mating_pool_.empty()) { 131 | size_t const idxA = rsl::uniform_int(0, mating_pool_.size() - 1); 132 | size_t idxB = idxA; 133 | while (idxB == idxA && mating_pool_.size() > 1) { 134 | idxB = rsl::uniform_int(0, mating_pool_.size() - 1); 135 | } 136 | auto& parentA = *(mating_pool_[idxA]); 137 | auto& parentB = *(mating_pool_[idxB]); 138 | 139 | // Get mutation probability 140 | double const extinction = 0.5 * (parentA.extinction + parentB.extinction); 141 | double const mutation_prob = 142 | extinction * (1.0 - inverse_gene_size_) + inverse_gene_size_; 143 | 144 | auto const mix_ratio = rsl::uniform_real(0.0, 1.0); 145 | for (size_t j_idx = 0; j_idx < robot.variables.size(); ++j_idx) { 146 | auto& gene = population_[i].genes[j_idx]; 147 | auto joint = robot.variables[j_idx]; 148 | 149 | // Reproduce 150 | gene = mix_ratio * parentA.genes[j_idx] + (1.0 - mix_ratio) * parentB.genes[j_idx]; 151 | 152 | // Add in parent gradients 153 | gene += rsl::uniform_real(0.0, 1.0) * parentA.gradient[j_idx] + 154 | rsl::uniform_real(0.0, 1.0) * parentB.gradient[j_idx]; 155 | auto original_gene = gene; 156 | 157 | // Mutate 158 | if (rsl::uniform_real(0.0, 1.0) < mutation_prob) { 159 | gene += extinction * joint.half_span * rsl::uniform_real(-1.0, 1.0); 160 | } 161 | 162 | // Clamp to valid joint values 163 | gene = robot.variables[j_idx].clamp_to_limits(gene); 164 | 165 | // Approximate gradient 166 | population_[i].gradient[j_idx] = gene - original_gene; 167 | } 168 | 169 | // Evaluate fitness and remove parents from the mating pool if a child with better 170 | // fitness exists. 171 | population_[i].fitness = cost_fn(population_[i].genes); 172 | if (population_[i].fitness < parentA.fitness) { 173 | auto it = std::find(mating_pool_.begin(), mating_pool_.end(), &parentA); 174 | if (it != mating_pool_.end()) mating_pool_.erase(it); 175 | } 176 | if (population_[i].fitness < parentB.fitness) { 177 | auto it = std::find(mating_pool_.begin(), mating_pool_.end(), &parentB); 178 | if (it != mating_pool_.end()) mating_pool_.erase(it); 179 | } 180 | 181 | } else { 182 | // If the mating pool is empty, roll a new population member randomly. 183 | robot.set_random_valid_configuration(population_[i].genes); 184 | population_[i].fitness = cost_fn(population_[i].genes); 185 | for (auto& g : population_[i].gradient) { 186 | g = 0.0; 187 | } 188 | } 189 | } 190 | } 191 | 192 | void MemeticIk::printPopulation() const { 193 | fmt::print("Fitnesses:\n"); 194 | for (size_t i = 0; i < populationCount(); ++i) { 195 | fmt::print("{}: {}\n", i, population_[i].fitness); 196 | } 197 | fmt::print("\n"); 198 | } 199 | 200 | void MemeticIk::sortPopulation() { 201 | std::sort(population_.begin(), population_.end(), [](Individual const& a, Individual const& b) { 202 | return a.fitness < b.fitness; 203 | }); 204 | computeExtinctions(); 205 | best_curr_ = population_[0]; 206 | if (best_curr_.fitness < best_.fitness) { 207 | best_ = best_curr_; 208 | } 209 | } 210 | 211 | auto ik_memetic_impl(std::vector const& initial_guess, 212 | Robot const& robot, 213 | CostFn const& cost_fn, 214 | SolutionTestFn const& solution_fn, 215 | MemeticIkParams const& params, 216 | std::atomic& terminate, 217 | bool approx_solution, 218 | bool print_debug) -> std::optional { 219 | assert(robot.variables.size() == initial_guess.size()); 220 | auto ik = MemeticIk::from(initial_guess, cost_fn, params); 221 | 222 | ik.initPopulation(robot, cost_fn, initial_guess); 223 | 224 | // Main loop 225 | int iter = 0; 226 | auto const timeout_point = 227 | std::chrono::system_clock::now() + std::chrono::duration(params.max_time); 228 | while ((std::chrono::system_clock::now() < timeout_point) && (iter < params.max_generations)) { 229 | // Do gradient descent on elites. 230 | std::vector gd_threads; 231 | gd_threads.reserve(ik.eliteCount()); 232 | for (size_t i = 0; i < ik.eliteCount(); ++i) { 233 | gd_threads.push_back(std::thread([&ik, i, &robot, cost_fn, ¶ms] { 234 | ik.gradientDescent(i, robot, cost_fn, params.gd_params); 235 | })); 236 | } 237 | for (auto& t : gd_threads) { 238 | t.join(); 239 | } 240 | 241 | // Perform mutation and recombination 242 | ik.reproduce(robot, cost_fn); 243 | 244 | // Sort fitnesses and update extinctions 245 | ik.sortPopulation(); 246 | if (print_debug) { 247 | fmt::print("Iteration {}\n", iter); 248 | ik.printPopulation(); 249 | } 250 | 251 | // Check for termination and wipeout conditions 252 | if (params.stop_optimization_on_valid_solution && solution_fn(ik.best().genes)) { 253 | if (print_debug) fmt::print("Found solution!\n"); 254 | return ik.best(); 255 | } 256 | if (ik.checkWipeout()) { 257 | // Ensure the first member of the new population is the best so far. 258 | if (print_debug) fmt::print("Population wipeout\n"); 259 | ik.initPopulation(robot, cost_fn, ik.best().genes); 260 | } 261 | 262 | // Check termination condition from other threads finding a solution. 263 | if (terminate) { 264 | if (print_debug) fmt::print("Terminated\n"); 265 | break; 266 | } 267 | 268 | iter++; 269 | } 270 | 271 | // If we kept optimizing, we need to check if we found a valid solution 272 | if (!params.stop_optimization_on_valid_solution && solution_fn(ik.best().genes)) { 273 | if (print_debug) fmt::print("Found solution!\n"); 274 | return ik.best(); 275 | } 276 | 277 | if (approx_solution) { 278 | if (print_debug) fmt::print("Returning best solution\n"); 279 | return ik.best(); 280 | } 281 | 282 | return std::nullopt; 283 | } 284 | 285 | auto ik_memetic(std::vector const& initial_guess, 286 | Robot const& robot, 287 | CostFn const& cost_fn, 288 | SolutionTestFn const& solution_fn, 289 | MemeticIkParams const& params, 290 | bool approx_solution, 291 | bool print_debug) -> std::optional> { 292 | // Check whether the initial guess already meets the goal, 293 | // before starting to solve. 294 | if (params.stop_optimization_on_valid_solution && solution_fn(initial_guess)) { 295 | return initial_guess; 296 | } 297 | 298 | std::atomic terminate{false}; 299 | if (params.num_threads <= 1) { 300 | // Single-threaded implementation 301 | auto maybe_solution = ik_memetic_impl(initial_guess, 302 | robot, 303 | cost_fn, 304 | solution_fn, 305 | params, 306 | terminate, 307 | approx_solution, 308 | print_debug); 309 | if (maybe_solution.has_value()) { 310 | return maybe_solution.value().genes; 311 | } 312 | } else { 313 | // Multi-threaded implementation 314 | rsl::Queue> solution_queue; 315 | std::vector ik_threads; 316 | ik_threads.reserve(params.num_threads); 317 | 318 | auto ik_thread_fn = [=, &terminate, &solution_queue]() { 319 | auto soln = ik_memetic_impl(initial_guess, 320 | robot, 321 | cost_fn, 322 | solution_fn, 323 | params, 324 | terminate, 325 | approx_solution, 326 | print_debug); 327 | solution_queue.push(soln); 328 | }; 329 | 330 | for (size_t i = 0; i < params.num_threads; ++i) { 331 | ik_threads.push_back(std::thread(ik_thread_fn)); 332 | } 333 | 334 | // If enabled, stop all other threads once one thread finds a valid solution. 335 | size_t n_threads_done = 0; 336 | std::vector best_solution; 337 | auto min_cost = std::numeric_limits::max(); 338 | auto maybe_solution = std::optional>{std::nullopt}; 339 | if (params.stop_on_first_soln) { 340 | while (!maybe_solution && (n_threads_done < params.num_threads)) { 341 | maybe_solution = solution_queue.pop(std::chrono::milliseconds(1)); 342 | } 343 | if (maybe_solution.value().has_value()) { 344 | auto const& solution = maybe_solution.value().value(); 345 | best_solution = solution.genes; 346 | min_cost = solution.fitness; 347 | terminate = true; 348 | } 349 | n_threads_done++; 350 | } 351 | 352 | for (auto& t : ik_threads) { 353 | t.join(); 354 | } 355 | 356 | // Get the minimum-cost solution from all threads. 357 | // Note that if approximate solutions are enabled, even if we terminate threads early, we 358 | // can still compare our first solution with the approximate ones from the other threads 359 | while (!solution_queue.empty()) { 360 | maybe_solution = solution_queue.pop(); 361 | if (maybe_solution.value().has_value()) { 362 | auto const& solution = maybe_solution.value().value(); 363 | auto const& cost = solution.fitness; 364 | if (cost < min_cost) { 365 | best_solution = solution.genes; 366 | min_cost = cost; 367 | } 368 | } 369 | } 370 | if (!best_solution.empty()) return best_solution; 371 | } 372 | return std::nullopt; 373 | } 374 | 375 | } // namespace pick_ik 376 | -------------------------------------------------------------------------------- /src/pick_ik_parameters.yaml: -------------------------------------------------------------------------------- 1 | pick_ik: 2 | # Overall solver settings 3 | mode: { 4 | type: string, 5 | default_value: "global", 6 | description: "IK solver mode. Set to global to allow the initial guess to be a long distance from the goal, or local if the initial guess is near the goal.", 7 | validation: { 8 | one_of<>: [["global", "local"]] 9 | } 10 | } 11 | gd_step_size: { 12 | type: double, 13 | default_value: 0.0001, 14 | description: "Gradient descent step size for joint perturbation", 15 | validation: { 16 | gt_eq<>: [1.0e-12], 17 | } 18 | } 19 | gd_max_iters: { 20 | type: int, 21 | default_value: 100, 22 | description: "Maximum iterations for local gradient descent", 23 | validation: { 24 | gt_eq<>: [1], 25 | } 26 | } 27 | gd_min_cost_delta: { 28 | type: double, 29 | default_value: 1.0e-12, 30 | description: "Minimum change in cost function value for gradient descent", 31 | validation: { 32 | gt_eq<>: [1.0e-64], 33 | } 34 | } 35 | # Cost functions and thresholds 36 | position_threshold: { 37 | type: double, 38 | default_value: 0.001, 39 | description: "Position threshold for solving IK, in meters", 40 | validation: { 41 | gt_eq<>: [0.0], 42 | }, 43 | } 44 | orientation_threshold: { 45 | type: double, 46 | default_value: 0.001, 47 | description: "Orientation threshold for solving IK, in radians", 48 | validation: { 49 | gt_eq<>: [0.0], 50 | }, 51 | } 52 | approximate_solution_position_threshold: { 53 | type: double, 54 | default_value: 0.05, 55 | description: "Position threshold for approximate IK solutions, in meters. If displacement is larger than this, the approximate solution will fall back to the initial guess", 56 | validation: { 57 | gt_eq<>: [0.0], 58 | }, 59 | } 60 | approximate_solution_orientation_threshold: { 61 | type: double, 62 | default_value: 0.05, 63 | description: "Orientation threshold for approximate IK solutions, in radians. If displacement is larger than this, the approximate solution will fall back to the initial guess", 64 | validation: { 65 | gt_eq<>: [0.0], 66 | }, 67 | } 68 | approximate_solution_joint_threshold: { 69 | type: double, 70 | default_value: 0.0, 71 | description: "Joint threshold for approximate IK solutions, in radians. If displacement is larger than this, the approximate solution will fall back to the initial guess", 72 | validation: { 73 | gt_eq<>: [0.0], 74 | }, 75 | } 76 | approximate_solution_cost_threshold: { 77 | type: double, 78 | default_value: 0.0, 79 | description: "Cost threshold for approximate IK solutions. If the result of the cost function is larger than this, the approximate solution will fall back to the initial guess.", 80 | validation: { 81 | gt_eq<>: [0.0], 82 | }, 83 | } 84 | cost_threshold: { 85 | type: double, 86 | default_value: 0.001, 87 | description: "Scalar value for comparing to result of cost functions. IK is considered solved when all position/rotation/twist thresholds are satisfied and all cost functions return a value lower than this value.", 88 | validation: { 89 | gt_eq<>: [0.0], 90 | }, 91 | } 92 | position_scale: { 93 | type: double, 94 | default_value: 1.0, 95 | description: "The position scale for the pose cost function. Set to 0.0 to solve for only rotation or other goals", 96 | validation: { 97 | gt_eq<>: [0.0], 98 | }, 99 | } 100 | rotation_scale: { 101 | type: double, 102 | default_value: 0.5, 103 | description: "The rotation scale for the pose cost function. Set to 0.0 to solve for only position", 104 | validation: { 105 | gt_eq<>: [0.0], 106 | }, 107 | } 108 | center_joints_weight: { 109 | type: double, 110 | default_value: 0.0, 111 | description: "Weight for centering cost function, >0.0 enables const function", 112 | validation: { 113 | gt_eq<>: [0.0], 114 | }, 115 | } 116 | avoid_joint_limits_weight: { 117 | type: double, 118 | default_value: 0.0, 119 | description: "Weight for avoiding joint limits cost function, >0.0 enables const function", 120 | validation: { 121 | gt_eq<>: [0.0], 122 | }, 123 | } 124 | minimal_displacement_weight: { 125 | type: double, 126 | default_value: 0.0, 127 | description: "Weight for minimal displacement cost function, >0.0 enables const function", 128 | validation: { 129 | gt_eq<>: [0.0], 130 | }, 131 | } 132 | stop_optimization_on_valid_solution : { 133 | type: bool, 134 | default_value: true, 135 | description: "If false, keeps running after finding a solution to further optimize the solution until a time or iteration limit is reached", 136 | } 137 | # Memetic IK specific parameters 138 | memetic_num_threads: { 139 | type: int, 140 | default_value: 1, 141 | description: "Number of threads for memetic IK", 142 | validation: { 143 | gt_eq<>: [1], 144 | } 145 | } 146 | memetic_stop_on_first_solution: { 147 | type: bool, 148 | default_value: true, 149 | description: "If true, stops on first solution and terminates other threads", 150 | } 151 | memetic_population_size: { 152 | type: int, 153 | default_value: 16, 154 | description: "Population size for memetic IK", 155 | validation: { 156 | gt_eq<>: [1], 157 | } 158 | } 159 | memetic_elite_size: { 160 | type: int, 161 | default_value: 4, 162 | description: "Number of elite members of memetic IK population", 163 | validation: { 164 | gt_eq<>: [1], 165 | } 166 | } 167 | memetic_wipeout_fitness_tol: { 168 | type: double, 169 | default_value: 0.00001, 170 | description: "Minimum fitness must improve by this value or population will be wiped out", 171 | validation: { 172 | gt_eq<>: [0.0], 173 | } 174 | } 175 | memetic_max_generations: { 176 | type: int, 177 | default_value: 100, 178 | description: "Maximum iterations of evolutionary algorithm", 179 | validation: { 180 | gt_eq<>: [1], 181 | } 182 | } 183 | memetic_gd_max_iters: { 184 | type: int, 185 | default_value: 25, 186 | description: "Maximum iterations of gradient descent during memetic exploitation", 187 | validation: { 188 | gt_eq<>: [1], 189 | } 190 | } 191 | memetic_gd_max_time: { 192 | type: double, 193 | default_value: 0.005, 194 | description: "Maximum time spent on gradient descent during memetic exploitation", 195 | validation: { 196 | gt_eq<>: [0.0], 197 | } 198 | } 199 | -------------------------------------------------------------------------------- /src/pick_ik_plugin.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace pick_ik { 18 | namespace { 19 | auto const LOGGER = rclcpp::get_logger("pick_ik"); 20 | } 21 | 22 | bool PickIKPlugin::initialize(rclcpp::Node::SharedPtr const& node, 23 | moveit::core::RobotModel const& robot_model, 24 | std::string const& group_name, 25 | std::string const& base_frame, 26 | std::vector const& tip_frames, 27 | double search_discretization) { 28 | node_ = node; 29 | parameter_listener_ = std::make_shared( 30 | node, 31 | std::string("robot_description_kinematics.").append(group_name)); 32 | 33 | // Initialize internal state of base class KinematicsBase 34 | // Creates these internal state variables: 35 | // robot_model_ <- shared_ptr to RobotModel 36 | // robot_description_ <- empty string 37 | // group_name_ <- group_name string 38 | // base_frame_ <- base_frame without leading / 39 | // tip_frames_ <- tip_frames without leading / 40 | // redundant_joint_discretization_ <- vector initialized with 41 | // search_discretization 42 | storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization); 43 | 44 | // Initialize internal state 45 | jmg_ = robot_model_->getJointModelGroup(group_name); 46 | if (!jmg_) { 47 | RCLCPP_ERROR(LOGGER, "failed to get joint model group %s", group_name.c_str()); 48 | return false; 49 | } 50 | 51 | // Joint names come from jmg 52 | for (auto* joint_model : jmg_->getJointModels()) { 53 | if (joint_model->getName() != base_frame_ && 54 | joint_model->getType() != moveit::core::JointModel::UNKNOWN && 55 | joint_model->getType() != moveit::core::JointModel::FIXED) { 56 | joint_names_.push_back(joint_model->getName()); 57 | } 58 | } 59 | 60 | // link_names are the same as tip frames 61 | // TODO: why do we need to set this 62 | link_names_ = tip_frames_; 63 | 64 | // Create our internal Robot object from the robot model 65 | tip_link_indices_ = get_link_indices(robot_model_, tip_frames_) 66 | .or_else([](auto const& error) { throw std::invalid_argument(error); }) 67 | .value(); 68 | robot_ = Robot::from(robot_model_, jmg_, tip_link_indices_); 69 | 70 | return true; 71 | } 72 | 73 | bool PickIKPlugin::searchPositionIK(std::vector const& ik_poses, 74 | std::vector const& ik_seed_state, 75 | double timeout, 76 | std::vector const&, 77 | std::vector& solution, 78 | IKCallbackFn const& solution_callback, 79 | IKCostFn const& cost_function, 80 | moveit_msgs::msg::MoveItErrorCodes& error_code, 81 | kinematics::KinematicsQueryOptions const& options, 82 | moveit::core::RobotState const* context_state) const { 83 | (void)context_state; // not used 84 | 85 | // Read current ROS parameters 86 | auto params = parameter_listener_->get_params(); 87 | 88 | auto const goal_frames = [&]() { 89 | auto robot_state = moveit::core::RobotState(robot_model_); 90 | robot_state.setToDefaultValues(); 91 | robot_state.setJointGroupPositions(jmg_, ik_seed_state); 92 | robot_state.update(); 93 | return transform_poses_to_frames(robot_state, ik_poses, getBaseFrame()); 94 | }(); 95 | 96 | // Test functions to determine if we are at our goal frame 97 | auto const test_position = (params.position_scale > 0); 98 | std::optional position_threshold = std::nullopt; 99 | if (test_position) { 100 | position_threshold = params.position_threshold; 101 | } 102 | auto const test_rotation = (params.rotation_scale > 0); 103 | std::optional orientation_threshold = std::nullopt; 104 | if (test_rotation) { 105 | orientation_threshold = params.orientation_threshold; 106 | } 107 | auto const frame_tests = 108 | make_frame_tests(goal_frames, position_threshold, orientation_threshold); 109 | 110 | // Cost functions used for optimizing towards goal frames 111 | auto const pose_cost_functions = 112 | make_pose_cost_functions(goal_frames, params.position_scale, params.rotation_scale); 113 | 114 | // forward kinematics function 115 | auto const fk_fn = make_fk_fn(robot_model_, jmg_, fk_mutex_, tip_link_indices_); 116 | 117 | // Create goals (weighted cost functions) 118 | auto goals = std::vector{}; 119 | if (params.center_joints_weight > 0.0) { 120 | goals.push_back(Goal{make_center_joints_cost_fn(robot_), params.center_joints_weight}); 121 | } 122 | if (params.avoid_joint_limits_weight > 0.0) { 123 | goals.push_back( 124 | Goal{make_avoid_joint_limits_cost_fn(robot_), params.avoid_joint_limits_weight}); 125 | } 126 | if (params.minimal_displacement_weight > 0.0) { 127 | goals.push_back(Goal{make_minimal_displacement_cost_fn(robot_, ik_seed_state), 128 | params.minimal_displacement_weight}); 129 | } 130 | if (cost_function) { 131 | for (auto const& pose : ik_poses) { 132 | goals.push_back( 133 | Goal{make_ik_cost_fn(pose, cost_function, robot_model_, jmg_, ik_seed_state), 1.0}); 134 | } 135 | } 136 | 137 | // test if this is a valid solution 138 | auto const solution_fn = 139 | make_is_solution_test_fn(frame_tests, goals, params.cost_threshold, fk_fn); 140 | 141 | // single function used by gradient descent to calculate cost of solution 142 | auto const cost_fn = make_cost_fn(pose_cost_functions, goals, fk_fn); 143 | 144 | // Set up initial optimization variables 145 | bool done_optimizing = false; 146 | bool found_valid_solution = false; 147 | double remaining_timeout = timeout; 148 | std::chrono::duration total_optim_time{0.0}; 149 | std::chrono::duration const total_timeout{timeout}; 150 | auto last_optim_time = std::chrono::system_clock::now(); 151 | 152 | // If the initial state is not valid, restart from a random valid state. 153 | auto init_state = ik_seed_state; 154 | if (!robot_.is_valid_configuration(init_state)) { 155 | RCLCPP_WARN( 156 | LOGGER, 157 | "Initial guess exceeds joint limits. Regenerating a random valid configuration."); 158 | robot_.set_random_valid_configuration(init_state); 159 | } 160 | 161 | // Optimize until a valid solution is found or we have timed out. 162 | while (!done_optimizing) { 163 | // Search for a solution using either the local or global solver. 164 | std::optional> maybe_solution; 165 | if (params.mode == "global") { 166 | MemeticIkParams ik_params; 167 | ik_params.population_size = static_cast(params.memetic_population_size); 168 | ik_params.elite_size = static_cast(params.memetic_elite_size); 169 | ik_params.wipeout_fitness_tol = params.memetic_wipeout_fitness_tol; 170 | ik_params.stop_optimization_on_valid_solution = 171 | params.stop_optimization_on_valid_solution; 172 | ik_params.num_threads = static_cast(params.memetic_num_threads); 173 | ik_params.stop_on_first_soln = params.memetic_stop_on_first_solution; 174 | ik_params.max_generations = static_cast(params.memetic_max_generations); 175 | ik_params.max_time = remaining_timeout; 176 | 177 | ik_params.gd_params.step_size = params.gd_step_size; 178 | ik_params.gd_params.min_cost_delta = params.gd_min_cost_delta; 179 | ik_params.gd_params.max_iterations = static_cast(params.memetic_gd_max_iters); 180 | ik_params.gd_params.max_time = params.memetic_gd_max_time; 181 | 182 | maybe_solution = ik_memetic(ik_seed_state, 183 | robot_, 184 | cost_fn, 185 | solution_fn, 186 | ik_params, 187 | options.return_approximate_solution, 188 | false /* No debug print */); 189 | } else if (params.mode == "local") { 190 | GradientIkParams gd_params; 191 | gd_params.step_size = params.gd_step_size; 192 | gd_params.min_cost_delta = params.gd_min_cost_delta; 193 | gd_params.max_time = remaining_timeout; 194 | gd_params.max_iterations = static_cast(params.gd_max_iters); 195 | gd_params.stop_optimization_on_valid_solution = 196 | params.stop_optimization_on_valid_solution; 197 | 198 | maybe_solution = ik_gradient(ik_seed_state, 199 | robot_, 200 | cost_fn, 201 | solution_fn, 202 | gd_params, 203 | options.return_approximate_solution); 204 | } else { 205 | RCLCPP_ERROR(LOGGER, "Invalid solver mode: %s", params.mode.c_str()); 206 | return false; 207 | } 208 | 209 | if (maybe_solution.has_value()) { 210 | // Set the output parameter solution. 211 | // Assumes that the angles were already wrapped by the solver. 212 | error_code.val = error_code.SUCCESS; 213 | solution = maybe_solution.value(); 214 | } else { 215 | error_code.val = error_code.NO_IK_SOLUTION; 216 | solution = ik_seed_state; 217 | } 218 | 219 | // If using an approximate solution, check against the maximum allowable pose and joint 220 | // thresholds. If the approximate solution is too far from the goal frame, 221 | // fall back to the initial state. 222 | if (options.return_approximate_solution) { 223 | // Check pose thresholds 224 | std::optional approximate_solution_position_threshold = std::nullopt; 225 | if (test_position) { 226 | approximate_solution_position_threshold = 227 | params.approximate_solution_position_threshold; 228 | } 229 | std::optional approximate_solution_orientation_threshold = std::nullopt; 230 | if (test_rotation) { 231 | approximate_solution_orientation_threshold = 232 | params.approximate_solution_orientation_threshold; 233 | } 234 | auto const approx_frame_tests = 235 | make_frame_tests(goal_frames, 236 | approximate_solution_position_threshold, 237 | approximate_solution_orientation_threshold); 238 | 239 | // If we have no cost threshold, we don't need to check the goals 240 | if (params.approximate_solution_cost_threshold <= 0.0) { 241 | goals.clear(); 242 | } 243 | 244 | auto const approx_solution_fn = 245 | make_is_solution_test_fn(frame_tests, 246 | goals, 247 | params.approximate_solution_cost_threshold, 248 | fk_fn); 249 | 250 | bool approx_solution_valid = approx_solution_fn(solution); 251 | 252 | // Check joint thresholds 253 | if (approx_solution_valid && params.approximate_solution_joint_threshold > 0.0) { 254 | for (size_t i = 0; i < solution.size(); ++i) { 255 | if (std::abs(solution[i] - ik_seed_state[i]) > 256 | params.approximate_solution_joint_threshold) { 257 | approx_solution_valid = false; 258 | break; 259 | } 260 | } 261 | } 262 | 263 | if (!approx_solution_valid) { 264 | error_code.val = error_code.NO_IK_SOLUTION; 265 | solution = ik_seed_state; 266 | } 267 | } 268 | 269 | // Execute solution callback only on successful solution. 270 | auto const found_solution = error_code.val == error_code.SUCCESS; 271 | if (solution_callback && found_solution) { 272 | solution_callback(ik_poses.front(), solution, error_code); 273 | } 274 | found_valid_solution = error_code.val == error_code.SUCCESS; 275 | 276 | // Check for timeout. 277 | auto const current_time = std::chrono::system_clock::now(); 278 | total_optim_time += (current_time - last_optim_time); 279 | last_optim_time = current_time; 280 | bool const timeout_elapsed = (total_optim_time >= total_timeout); 281 | 282 | // If we found a valid solution or hit the timeout, we are done optimizing. 283 | // Otherwise, pick a random new initial seed and keep optimizing with the remaining 284 | // time. 285 | if (found_valid_solution || timeout_elapsed) { 286 | done_optimizing = true; 287 | } else { 288 | robot_.set_random_valid_configuration(init_state); 289 | remaining_timeout = timeout - total_optim_time.count(); 290 | } 291 | } 292 | 293 | return found_valid_solution; 294 | } 295 | 296 | std::vector const& PickIKPlugin::getJointNames() const { return joint_names_; } 297 | 298 | std::vector const& PickIKPlugin::getLinkNames() const { return link_names_; } 299 | 300 | bool PickIKPlugin::getPositionFK(std::vector const&, 301 | std::vector const&, 302 | std::vector&) const { 303 | return false; 304 | } 305 | 306 | bool PickIKPlugin::getPositionIK(geometry_msgs::msg::Pose const&, 307 | std::vector const&, 308 | std::vector&, 309 | moveit_msgs::msg::MoveItErrorCodes&, 310 | kinematics::KinematicsQueryOptions const&) const { 311 | return false; 312 | } 313 | 314 | bool PickIKPlugin::searchPositionIK(geometry_msgs::msg::Pose const& ik_pose, 315 | std::vector const& ik_seed_state, 316 | double timeout, 317 | std::vector& solution, 318 | moveit_msgs::msg::MoveItErrorCodes& error_code, 319 | kinematics::KinematicsQueryOptions const& options) const { 320 | return searchPositionIK(std::vector{ik_pose}, 321 | ik_seed_state, 322 | timeout, 323 | std::vector(), 324 | solution, 325 | IKCallbackFn(), 326 | error_code, 327 | options); 328 | } 329 | 330 | bool PickIKPlugin::searchPositionIK(geometry_msgs::msg::Pose const& ik_pose, 331 | std::vector const& ik_seed_state, 332 | double timeout, 333 | std::vector const& consistency_limits, 334 | std::vector& solution, 335 | moveit_msgs::msg::MoveItErrorCodes& error_code, 336 | kinematics::KinematicsQueryOptions const& options) const { 337 | return searchPositionIK(std::vector{ik_pose}, 338 | ik_seed_state, 339 | timeout, 340 | consistency_limits, 341 | solution, 342 | IKCallbackFn(), 343 | error_code, 344 | options); 345 | } 346 | 347 | bool PickIKPlugin::searchPositionIK(geometry_msgs::msg::Pose const& ik_pose, 348 | std::vector const& ik_seed_state, 349 | double timeout, 350 | std::vector& solution, 351 | IKCallbackFn const& solution_callback, 352 | moveit_msgs::msg::MoveItErrorCodes& error_code, 353 | kinematics::KinematicsQueryOptions const& options) const { 354 | return searchPositionIK(std::vector{ik_pose}, 355 | ik_seed_state, 356 | timeout, 357 | std::vector(), 358 | solution, 359 | solution_callback, 360 | error_code, 361 | options); 362 | } 363 | 364 | bool PickIKPlugin::searchPositionIK(geometry_msgs::msg::Pose const& ik_pose, 365 | std::vector const& ik_seed_state, 366 | double timeout, 367 | std::vector const& consistency_limits, 368 | std::vector& solution, 369 | IKCallbackFn const& solution_callback, 370 | moveit_msgs::msg::MoveItErrorCodes& error_code, 371 | kinematics::KinematicsQueryOptions const& options) const { 372 | return searchPositionIK(std::vector{ik_pose}, 373 | ik_seed_state, 374 | timeout, 375 | consistency_limits, 376 | solution, 377 | solution_callback, 378 | error_code, 379 | options); 380 | } 381 | 382 | bool PickIKPlugin::searchPositionIK(std::vector const& ik_poses, 383 | std::vector const& ik_seed_state, 384 | double timeout, 385 | std::vector const& consistency_limits, 386 | std::vector& solution, 387 | IKCallbackFn const& solution_callback, 388 | moveit_msgs::msg::MoveItErrorCodes& error_code, 389 | kinematics::KinematicsQueryOptions const& options, 390 | moveit::core::RobotState const* context_state) const { 391 | return searchPositionIK(ik_poses, 392 | ik_seed_state, 393 | timeout, 394 | consistency_limits, 395 | solution, 396 | solution_callback, 397 | IKCostFn(), 398 | error_code, 399 | options, 400 | context_state); 401 | } 402 | 403 | } // namespace pick_ik 404 | 405 | PLUGINLIB_EXPORT_CLASS(pick_ik::PickIKPlugin, kinematics::KinematicsBase); 406 | -------------------------------------------------------------------------------- /src/robot.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | namespace { 17 | constexpr double kUnboundedVariableHalfSpan = M_PI; 18 | constexpr double kUnboundedJointSampleSpread = M_PI; 19 | } // namespace 20 | 21 | namespace pick_ik { 22 | 23 | auto Robot::Variable::generate_valid_value(double init_val /* = 0.0*/) const -> double { 24 | if (bounded) { 25 | return rsl::uniform_real(min, max); 26 | } else { 27 | return rsl::uniform_real(init_val - kUnboundedJointSampleSpread, 28 | init_val + kUnboundedJointSampleSpread); 29 | } 30 | } 31 | 32 | auto Robot::Variable::is_valid(double val) const -> bool { 33 | return (!bounded) || (val <= max && val >= min); 34 | } 35 | 36 | auto Robot::Variable::clamp_to_limits(double val) const -> double { 37 | if (bounded) { 38 | return std::clamp(val, min, max); 39 | } else { 40 | return std::clamp(val, val - half_span, val + half_span); 41 | } 42 | } 43 | 44 | auto Robot::from(std::shared_ptr const& model, 45 | moveit::core::JointModelGroup const* jmg, 46 | std::vector tip_link_indices) -> Robot { 47 | auto robot = Robot{}; 48 | 49 | auto const active_variable_indices = get_active_variable_indices(model, jmg, tip_link_indices); 50 | auto const variable_count = active_variable_indices.size(); 51 | 52 | auto const& names = model->getVariableNames(); 53 | 54 | auto minimal_displacement_divisor = 0.0; 55 | 56 | for (auto ivar : active_variable_indices) { 57 | auto const& name = names.at(ivar); 58 | auto const& bounds = model->getVariableBounds(name); 59 | 60 | auto var = Variable{}; 61 | 62 | var.bounded = bounds.position_bounded_; 63 | var.min = bounds.min_position_; 64 | var.max = bounds.max_position_; 65 | var.mid = 0.5 * (var.min + var.max); 66 | var.half_span = var.bounded ? (var.max - var.min) / 2.0 : kUnboundedVariableHalfSpan; 67 | 68 | auto const max_velocity = bounds.max_velocity_; 69 | var.max_velocity_rcp = max_velocity > 0.0 ? 1.0 / max_velocity : 0.0; 70 | 71 | var.minimal_displacement_factor = 1.0 / static_cast(variable_count); 72 | minimal_displacement_divisor += var.max_velocity_rcp; 73 | 74 | robot.variables.push_back(var); 75 | } 76 | 77 | // Calculate minimal displacement factors 78 | if (minimal_displacement_divisor > 0) { 79 | for (auto& var : robot.variables) { 80 | var.minimal_displacement_factor = var.max_velocity_rcp / minimal_displacement_divisor; 81 | } 82 | } 83 | 84 | return robot; 85 | } 86 | 87 | auto Robot::set_random_valid_configuration(std::vector& config) const -> void { 88 | auto const num_vars = variables.size(); 89 | if (config.size() != num_vars) { 90 | config.resize(num_vars); 91 | } 92 | for (size_t idx = 0; idx < num_vars; ++idx) { 93 | config[idx] = variables[idx].generate_valid_value(config[idx]); 94 | } 95 | } 96 | 97 | auto Robot::is_valid_configuration(std::vector const& config) const -> bool { 98 | auto const num_vars = variables.size(); 99 | for (size_t idx = 0; idx < num_vars; ++idx) { 100 | if (!variables[idx].is_valid(config[idx])) { 101 | return false; 102 | } 103 | } 104 | return true; 105 | } 106 | 107 | auto get_link_indices(std::shared_ptr const& model, 108 | std::vector const& names) 109 | -> tl::expected, std::string> { 110 | auto indices = std::vector(); 111 | for (auto const& name : names) { 112 | auto const* link_model = model->getLinkModel(name); 113 | if (!link_model) { 114 | return tl::make_unexpected(fmt::format("link not found: {}", name)); 115 | } 116 | indices.push_back(link_model->getLinkIndex()); 117 | } 118 | 119 | return indices; 120 | } 121 | 122 | auto get_active_variable_indices(std::shared_ptr const& robot_model, 123 | moveit::core::JointModelGroup const* jmg, 124 | std::vector const& tip_link_indices) 125 | -> std::vector { 126 | // Walk the tree of links starting at each tip towards the parent 127 | // The parent joint of each of these links are the ones we are using 128 | auto joint_usage = std::vector{}; 129 | joint_usage.resize(robot_model->getJointModelCount(), 0); 130 | for (auto tip_index : tip_link_indices) { 131 | for (auto const* link_model = robot_model->getLinkModels().at(tip_index); 132 | link_model != nullptr; 133 | link_model = link_model->getParentLinkModel()) { 134 | auto const* joint_model = link_model->getParentJointModel(); 135 | auto const joint_index = joint_model->getJointIndex(); 136 | joint_usage[joint_index] = 1; 137 | } 138 | } 139 | 140 | // For each of the active joints in the joint model group 141 | // If those are in the ones we are using and the joint is not a mimic 142 | // Then get all the variable names from the joint moodel 143 | auto active_variable_names = std::vector{}; 144 | for (auto const* joint_model : jmg->getActiveJointModels()) { 145 | if (joint_usage[joint_model->getJointIndex()] && !joint_model->getMimic()) { 146 | for (auto& name : joint_model->getVariableNames()) { 147 | active_variable_names.push_back(name); 148 | } 149 | } 150 | } 151 | 152 | // For each active variable name, add the indices from that variable to the 153 | // active variables 154 | auto active_variable_indices = std::vector{}; 155 | for (auto const& name : active_variable_names) { 156 | active_variable_indices.push_back(robot_model->getVariableIndex(name)); 157 | } 158 | 159 | return active_variable_indices; 160 | } 161 | 162 | auto get_variables(moveit::core::RobotState const& robot_state) -> std::vector { 163 | auto const count = robot_state.getRobotModel()->getVariableCount(); 164 | auto const* data = robot_state.getVariablePositions(); 165 | std::vector variables(data, data + count); 166 | return variables; 167 | } 168 | 169 | auto transform_poses_to_frames(moveit::core::RobotState const& robot_state, 170 | std::vector const& poses, 171 | std::string const& base_frame_name) 172 | -> std::vector { 173 | auto frames = std::vector{}; 174 | std::transform(poses.cbegin(), poses.cend(), std::back_inserter(frames), [&](auto const& pose) { 175 | Eigen::Isometry3d p; 176 | tf2::fromMsg(pose, p); 177 | auto const r = robot_state.getGlobalLinkTransform(base_frame_name); 178 | return (r * p); 179 | }); 180 | return frames; 181 | } 182 | 183 | } // namespace pick_ik 184 | -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 2 | 3 | find_package(Catch2 3.3.0 REQUIRED) 4 | 5 | add_executable(test-pick_ik 6 | goal_tests.cpp 7 | ik_tests.cpp 8 | ik_memetic_tests.cpp 9 | robot_tests.cpp 10 | ) 11 | target_link_libraries(test-pick_ik 12 | PRIVATE 13 | pick_ik_plugin 14 | Catch2::Catch2WithMain 15 | moveit_core::moveit_test_utils 16 | ) 17 | catch_discover_tests(test-pick_ik) 18 | -------------------------------------------------------------------------------- /tests/goal_tests.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | TEST_CASE("pick_ik::make_frame_tests") { 10 | auto const position_epsilon = 0.00001; 11 | auto const orientation_epsilon = 0.001; 12 | auto const zero_frame = Eigen::Isometry3d::Identity(); 13 | 14 | SECTION("One goal, one test") { 15 | auto const test_fns = 16 | pick_ik::make_frame_tests({zero_frame}, position_epsilon, orientation_epsilon); 17 | CHECK(test_fns.size() == 1); 18 | } 19 | 20 | SECTION("Zero threshold") { 21 | auto const test_fns = pick_ik::make_frame_tests({zero_frame}, 0.0, 0.0); 22 | CHECK(test_fns.at(0)({zero_frame}) == true); 23 | } 24 | 25 | SECTION("Goal is almost frame, but not quite") { 26 | Eigen::Isometry3d const frame = 27 | Eigen::Translation3d(position_epsilon, position_epsilon, position_epsilon) * 28 | Eigen::Quaterniond(1 - orientation_epsilon, 0.0, 0.0, orientation_epsilon); 29 | 30 | auto const test_fns = 31 | pick_ik::make_frame_tests({zero_frame}, position_epsilon, orientation_epsilon); 32 | CHECK(test_fns.at(0)({frame}) == false); 33 | } 34 | 35 | SECTION("Goal is almost frame, within position but not orientation threshold") { 36 | Eigen::Isometry3d const frame = 37 | Eigen::Translation3d(0.0, 0.000009, 0.0) * Eigen::Quaterniond(0.707, 0.0, 0.707, 0.0); 38 | 39 | auto const test_fns = 40 | pick_ik::make_frame_tests({zero_frame}, position_epsilon, orientation_epsilon); 41 | CHECK(test_fns.at(0)({frame}) == false); 42 | } 43 | 44 | SECTION("Goal is almost frame, within threshold") { 45 | Eigen::Isometry3d const frame = Eigen::Translation3d(0.0, 0.000009, 0.0) * 46 | Eigen::Quaterniond(0.99999, 0.0, 0.0, 0.00001); 47 | 48 | auto const test_fns = 49 | pick_ik::make_frame_tests({zero_frame}, position_epsilon, orientation_epsilon); 50 | CHECK(test_fns.at(0)({frame}) == true); 51 | } 52 | 53 | SECTION("Goal is frame") { 54 | auto const test_fns = 55 | pick_ik::make_frame_tests({zero_frame}, position_epsilon, orientation_epsilon); 56 | CHECK(test_fns.at(0)({zero_frame}) == true); 57 | } 58 | 59 | SECTION("Goal is frame, but orientation is different") { 60 | auto const zero_frame_rotated = Eigen::Translation3d(0.0, 0.0, 0.0) * 61 | Eigen::AngleAxisd(M_PI_4, Eigen::Vector3d::UnitZ()); 62 | 63 | // With rotation in frame test 64 | auto const test_fns = 65 | pick_ik::make_frame_tests({zero_frame}, position_epsilon, orientation_epsilon); 66 | CHECK(test_fns.at(0)({zero_frame_rotated}) == false); 67 | 68 | // Without rotation in frame test 69 | auto const test_fns_pos_only = pick_ik::make_frame_tests({zero_frame}, position_epsilon); 70 | CHECK(test_fns_pos_only.at(0)({zero_frame_rotated}) == true); 71 | } 72 | } 73 | 74 | TEST_CASE("pick_ik::make_pose_cost_fn") { 75 | auto const zero_frame = Eigen::Isometry3d::Identity(); 76 | Eigen::Isometry3d const translate_y2_frame = 77 | Eigen::Translation3d(0.0, 2.0, 0.0) * Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); 78 | Eigen::Isometry3d const translate_xy1_frame = 79 | Eigen::Translation3d(1.0, 1.0, 0.0) * Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); 80 | Eigen::Isometry3d const translate_xyz1_frame = 81 | Eigen::Translation3d(1.0, 1.0, 1.0) * Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); 82 | Eigen::Isometry3d const rotate_x1_frame = 83 | Eigen::Translation3d(0.0, 0.0, 0.0) * Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX()); 84 | Eigen::Isometry3d const rotate_y2_frame = 85 | Eigen::Translation3d(0.0, 0.0, 0.0) * Eigen::AngleAxisd(2.0, Eigen::Vector3d::UnitY()); 86 | 87 | SECTION("Goal is frame") { 88 | auto const cost_fn = pick_ik::make_pose_cost_fn(zero_frame, 0, 0.0, 0.0); 89 | CHECK(cost_fn({zero_frame}) == Catch::Approx(0.0)); 90 | } 91 | 92 | SECTION("Goal is frame, with position scale") { 93 | auto const cost_fn = pick_ik::make_pose_cost_fn(zero_frame, 0, 1.0, 0.0); 94 | CHECK(cost_fn({zero_frame}) == Catch::Approx(0.0)); 95 | } 96 | 97 | SECTION("Goal is frame, with rotation scale") { 98 | auto const cost_fn = pick_ik::make_pose_cost_fn(zero_frame, 0, 1.0, 0.5); 99 | CHECK(cost_fn({zero_frame}) == Catch::Approx(0.0)); 100 | } 101 | 102 | SECTION("Goal is second index") { 103 | auto const cost_fn = pick_ik::make_pose_cost_fn(translate_y2_frame, 1, 1.0, 0.0); 104 | CHECK(cost_fn({zero_frame, translate_y2_frame}) == Catch::Approx(0.0)); 105 | } 106 | 107 | SECTION("Translation along one axis, square of distance") { 108 | auto const cost_fn = pick_ik::make_pose_cost_fn(zero_frame, 0, 1.0, 0.5); 109 | CHECK(cost_fn({translate_y2_frame}) == Catch::Approx(std::pow(2.0, 2))); 110 | } 111 | 112 | SECTION("Translation in two axes, square of distance") { 113 | // We know that the distance between (0,0,0) and (1,1,0) is sqrt(2), 114 | // so the squared distance in the cost function should be 2. 115 | auto const const_fn = pick_ik::make_pose_cost_fn(zero_frame, 0, 1.0, 0.5); 116 | CHECK(const_fn({translate_xy1_frame}) == Catch::Approx(2.0)); 117 | } 118 | 119 | SECTION("Translation in three axes, square of distance") { 120 | // We know that the distance between (0,0,0) and (1,1,1) is sqrt(3), 121 | // so the squared distance in the cost function should be 3. 122 | auto const const_fn = pick_ik::make_pose_cost_fn(zero_frame, 0, 1.0, 0.5); 123 | CHECK(const_fn({translate_xyz1_frame}) == Catch::Approx(3.0)); 124 | } 125 | 126 | SECTION("Zero position scale with translation") { 127 | auto const cost_fn = pick_ik::make_pose_cost_fn(zero_frame, 0, 0.0, 0.5); 128 | CHECK(cost_fn({translate_xyz1_frame}) == Catch::Approx(0.0)); 129 | } 130 | 131 | SECTION("Zero rotation scale with rotation") { 132 | auto const cost_fn = pick_ik::make_pose_cost_fn(zero_frame, 0, 1.0, 0.0); 133 | CHECK(cost_fn({rotate_x1_frame}) == Catch::Approx(0.0)); 134 | } 135 | 136 | SECTION("Negative position scale same as zero position scale") { 137 | auto const cost_fn_zero = pick_ik::make_pose_cost_fn(zero_frame, 0, 0.0, 0.5); 138 | auto const cost_fn_neg = pick_ik::make_pose_cost_fn(zero_frame, 0, -1.0, 0.5); 139 | CHECK(cost_fn_zero({translate_xyz1_frame}) == cost_fn_neg({translate_xyz1_frame})); 140 | } 141 | 142 | SECTION("Negative rotation scale same as zero rotation scale") { 143 | auto const cost_fn_zero = pick_ik::make_pose_cost_fn(zero_frame, 0, 1.0, 0.0); 144 | auto const cost_fn_neg = pick_ik::make_pose_cost_fn(zero_frame, 0, 1.0, -0.5); 145 | CHECK(cost_fn_zero({rotate_x1_frame}) == cost_fn_neg({rotate_x1_frame})); 146 | } 147 | 148 | SECTION("Rotation in one axis") { 149 | // Since we specified an angle of 2 radians about the Y axis, the 150 | // squared angle should be 4.0. 151 | auto const rotational_distance = 2.0; 152 | auto const cost_fn = pick_ik::make_pose_cost_fn(zero_frame, 0, 1.0, 1.0); 153 | CHECK(cost_fn({rotate_y2_frame}) == Catch::Approx(std::pow(rotational_distance, 2))); 154 | } 155 | 156 | SECTION("Rotation in one axis, scaled 0.5") { 157 | // Since we specified an angle of 2 radians about the Y axis, the 158 | // squared angle should be 4.0. 159 | auto const rotational_distance = 2.0; 160 | auto const position_scale = 1.0; 161 | auto const rotation_scale = 0.5; 162 | 163 | auto const cost_fn = 164 | pick_ik::make_pose_cost_fn(zero_frame, 0, position_scale, rotation_scale); 165 | 166 | // The rotation scale is squared in addition to the square of the distance 167 | CHECK(cost_fn({rotate_y2_frame}) == 168 | Catch::Approx(std::pow(rotational_distance, 2) * std::pow(rotation_scale, 2))); 169 | } 170 | 171 | SECTION("Test 0 - sample data taken from instrumenting bio_ik") { 172 | auto const q_goal = Eigen::Quaterniond(3.2004117980888137e-12, 173 | 0.9239557003781338, 174 | -0.38249949508300274, 175 | 1.324932598914536e-12); 176 | Eigen::Isometry3d const goal = 177 | Eigen::Translation3d(0.3548182547092438, -0.04776066541671753, 0.5902695655822754) * 178 | q_goal; 179 | 180 | auto const q_frame = Eigen::Quaterniond(-0.0033032628064278945, 181 | 0.9163043570028795, 182 | -0.40044067474764505, 183 | -0.004762331364117075); 184 | Eigen::Isometry3d const frame = 185 | Eigen::Translation3d(0.3363926217416014, -0.043807946580255344, 0.5864240526436293) * 186 | q_frame; 187 | 188 | auto const position_scale = 1.0; 189 | auto const rotation_scale = 0.5; 190 | auto const expected_cost = 191 | (goal.translation() - frame.translation()).squaredNorm() + 192 | std::pow(2.0 * std::acos(q_goal.dot(q_frame)) * rotation_scale, 2); 193 | 194 | auto const cost_fn = pick_ik::make_pose_cost_fn(goal, 0, position_scale, rotation_scale); 195 | auto const cost = cost_fn({frame}); 196 | CHECK(cost == Catch::Approx(expected_cost)); 197 | } 198 | 199 | SECTION("Test 2 - sample data taken from instrumenting bio_ik") { 200 | auto const q_goal = Eigen::Quaterniond(3.2004117980888137e-12, 201 | 0.9239557003781338, 202 | -0.38249949508300274, 203 | 1.324932598914536e-12); 204 | Eigen::Isometry3d const goal = 205 | Eigen::Translation3d(0.3327501714229584, -0.025710120797157288, 0.5902695655822754) * 206 | q_goal; 207 | 208 | auto const q_frame = Eigen::Quaterniond(2.1223489422435532e-07, 209 | 0.9239554647443051, 210 | -0.38250006378889556, 211 | 1.925047999919496e-05); 212 | Eigen::Isometry3d const frame = 213 | Eigen::Translation3d(0.3327318727877646, -0.02570328270961634, 0.5900141633600922) * 214 | q_frame; 215 | 216 | auto const position_scale = 1.0; 217 | auto const rotation_scale = 0.5; 218 | auto const expected_cost = 219 | (goal.translation() - frame.translation()).squaredNorm() + 220 | std::pow(2.0 * std::acos(q_goal.dot(q_frame)) * rotation_scale, 2); 221 | 222 | auto const cost_fn = pick_ik::make_pose_cost_fn(goal, 0, position_scale, rotation_scale); 223 | auto const cost = cost_fn({frame}); 224 | CHECK(cost == Catch::Approx(expected_cost)); 225 | } 226 | } 227 | 228 | TEST_CASE("pick_ik::make_pose_cost_functions") { 229 | Eigen::Isometry3d const goal = 230 | Eigen::Translation3d(0.3327501714229584, -0.025710120797157288, 0.5902695655822754) * 231 | Eigen::Quaterniond(3.2004117980888137e-12, 232 | 0.9239557003781338, 233 | -0.38249949508300274, 234 | 1.324932598914536e-12); 235 | Eigen::Isometry3d const frame = 236 | Eigen::Translation3d(0.3327318727877646, -0.02570328270961634, 0.5900141633600922) * 237 | Eigen::Quaterniond(2.1223489422435532e-07, 238 | 0.9239554647443051, 239 | -0.38250006378889556, 240 | 1.925047999919496e-05); 241 | auto const position_scale = 1.0; 242 | auto const rotation_scale = 0.5; 243 | 244 | SECTION("Function is same as pick_ik::make_pose_cost_fn") { 245 | auto const cost_fn = pick_ik::make_pose_cost_fn(goal, 0, position_scale, rotation_scale); 246 | auto const cost_fns = 247 | pick_ik::make_pose_cost_functions({goal}, position_scale, rotation_scale); 248 | 249 | CHECK(cost_fn({frame}) == cost_fns.at(0)({frame})); 250 | } 251 | 252 | SECTION("One goal, one function") { 253 | auto const cost_fns = 254 | pick_ik::make_pose_cost_functions({goal}, position_scale, rotation_scale); 255 | CHECK(cost_fns.size() == 1); 256 | } 257 | 258 | SECTION("Two goals, two functions") { 259 | auto const cost_fns = 260 | pick_ik::make_pose_cost_functions({goal, frame}, position_scale, rotation_scale); 261 | CHECK(cost_fns.size() == 2); 262 | } 263 | 264 | SECTION("First goal, tests first frame") { 265 | auto const cost_fns = 266 | pick_ik::make_pose_cost_functions({goal, frame}, position_scale, rotation_scale); 267 | CHECK(cost_fns.at(0)({goal, frame}) == Catch::Approx(0.0).margin(1e-15)); 268 | } 269 | 270 | SECTION("Second goal, tests second frame") { 271 | auto const cost_fns = 272 | pick_ik::make_pose_cost_functions({goal, frame}, position_scale, rotation_scale); 273 | CHECK(cost_fns.at(1)({goal, frame}) == Catch::Approx(0.0).margin(1e-15)); 274 | } 275 | } 276 | -------------------------------------------------------------------------------- /tests/ik_memetic_tests.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | // Helper param struct and function to test IK solution. 17 | struct MemeticIkTestParams { 18 | double position_threshold = 0.001; 19 | double orientation_threshold = 0.01; 20 | double cost_threshold = 0.001; 21 | double position_scale = 1.0; 22 | double rotation_scale = 0.5; 23 | 24 | // Solve options 25 | bool approximate_solution = false; 26 | bool print_debug = false; 27 | pick_ik::MemeticIkParams memetic_params; 28 | 29 | // Additional costs 30 | double center_joints_weight = 0.0; 31 | double avoid_joint_limits_weight = 0.0; 32 | double minimal_displacement_weight = 0.0; 33 | }; 34 | 35 | auto solve_memetic_ik_test(moveit::core::RobotModelPtr robot_model, 36 | std::string const group_name, 37 | std::string const goal_frame_name, 38 | Eigen::Isometry3d const& goal_frame, 39 | std::vector const& initial_guess, 40 | MemeticIkTestParams const& params = MemeticIkTestParams()) 41 | -> std::optional> { 42 | // Make forward kinematics function 43 | auto const jmg = robot_model->getJointModelGroup(group_name); 44 | auto const tip_link_indices = pick_ik::get_link_indices(robot_model, {goal_frame_name}).value(); 45 | std::mutex mx; 46 | auto const fk_fn = pick_ik::make_fk_fn(robot_model, jmg, mx, tip_link_indices); 47 | auto const robot = pick_ik::Robot::from(robot_model, jmg, tip_link_indices); 48 | 49 | // Make goal function(s) 50 | std::vector goals = {}; 51 | if (params.center_joints_weight > 0) { 52 | goals.push_back( 53 | pick_ik::Goal{pick_ik::make_center_joints_cost_fn(robot), params.center_joints_weight}); 54 | } 55 | if (params.avoid_joint_limits_weight > 0) { 56 | goals.push_back(pick_ik::Goal{pick_ik::make_avoid_joint_limits_cost_fn(robot), 57 | params.center_joints_weight}); 58 | } 59 | if (params.minimal_displacement_weight > 0) { 60 | goals.push_back( 61 | pick_ik::Goal{pick_ik::make_minimal_displacement_cost_fn(robot, initial_guess), 62 | params.center_joints_weight}); 63 | } 64 | 65 | // Make pose cost function 66 | auto const pose_cost_functions = pick_ik::make_pose_cost_functions({goal_frame}, 67 | params.position_scale, 68 | params.rotation_scale); 69 | CHECK(pose_cost_functions.size() == 1); 70 | auto const cost_fn = pick_ik::make_cost_fn(pose_cost_functions, goals, fk_fn); 71 | 72 | // Make solution function 73 | auto const test_position = (params.position_scale > 0); 74 | std::optional position_threshold = std::nullopt; 75 | if (test_position) { 76 | position_threshold = params.position_threshold; 77 | } 78 | auto const test_rotation = (params.rotation_scale > 0.0); 79 | std::optional orientation_threshold = std::nullopt; 80 | if (test_rotation) { 81 | orientation_threshold = params.orientation_threshold; 82 | } 83 | auto const frame_tests = 84 | pick_ik::make_frame_tests({goal_frame}, position_threshold, orientation_threshold); 85 | auto const solution_fn = 86 | pick_ik::make_is_solution_test_fn(frame_tests, goals, params.cost_threshold, fk_fn); 87 | 88 | // Solve memetic IK 89 | return pick_ik::ik_memetic(initial_guess, 90 | robot, 91 | cost_fn, 92 | solution_fn, 93 | params.memetic_params, 94 | params.approximate_solution, 95 | params.print_debug); 96 | } 97 | 98 | TEST_CASE("Panda model Memetic IK") { 99 | using moveit::core::loadTestingRobotModel; 100 | auto const robot_model = loadTestingRobotModel("panda"); 101 | 102 | auto const jmg = robot_model->getJointModelGroup("panda_arm"); 103 | auto const tip_link_indices = pick_ik::get_link_indices(robot_model, {"panda_hand"}).value(); 104 | std::mutex mx; 105 | auto const fk_fn = pick_ik::make_fk_fn(robot_model, jmg, mx, tip_link_indices); 106 | 107 | std::vector const home_joint_angles = 108 | {0.0, -M_PI_4, 0.0, -3.0 * M_PI_4, 0.0, M_PI_2, M_PI_4}; 109 | 110 | SECTION("Panda model IK at home positions.") { 111 | auto const goal_frame = fk_fn(home_joint_angles)[0]; 112 | auto initial_guess = home_joint_angles; 113 | MemeticIkTestParams params; 114 | 115 | auto const maybe_solution = solve_memetic_ik_test(robot_model, 116 | "panda_arm", 117 | "panda_hand", 118 | goal_frame, 119 | initial_guess, 120 | params); 121 | 122 | REQUIRE(maybe_solution.has_value()); 123 | auto const final_frame = fk_fn(maybe_solution.value())[0]; 124 | CHECK(goal_frame.isApprox(final_frame, params.position_threshold)); 125 | } 126 | 127 | SECTION("Panda model IK near home positions.") { 128 | auto const goal_frame = fk_fn(home_joint_angles)[0]; 129 | auto initial_guess = home_joint_angles; 130 | std::vector const initial_guess_offsets = {0.1, -0.1, 0.0, 0.1, -0.1, 0.0, 0.1}; 131 | for (size_t i = 0; i < initial_guess.size(); ++i) { 132 | initial_guess[i] += initial_guess_offsets[i]; 133 | } 134 | MemeticIkTestParams params; 135 | 136 | auto const maybe_solution = solve_memetic_ik_test(robot_model, 137 | "panda_arm", 138 | "panda_hand", 139 | goal_frame, 140 | initial_guess, 141 | params); 142 | 143 | REQUIRE(maybe_solution.has_value()); 144 | auto const final_frame = fk_fn(maybe_solution.value())[0]; 145 | CHECK(goal_frame.isApprox(final_frame, params.position_threshold)); 146 | } 147 | 148 | SECTION("Panda model IK at zero positions -- single threaded") { 149 | auto const goal_frame = fk_fn(home_joint_angles)[0]; 150 | auto const initial_guess = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; 151 | MemeticIkTestParams params; 152 | params.print_debug = false; 153 | 154 | auto const maybe_solution = solve_memetic_ik_test(robot_model, 155 | "panda_arm", 156 | "panda_hand", 157 | goal_frame, 158 | initial_guess, 159 | params); 160 | 161 | REQUIRE(maybe_solution.has_value()); 162 | auto const final_frame = fk_fn(maybe_solution.value())[0]; 163 | CHECK(goal_frame.isApprox(final_frame, params.position_threshold)); 164 | } 165 | 166 | SECTION("Panda model IK at zero positions -- multithreaded") { 167 | auto const goal_frame = fk_fn(home_joint_angles)[0]; 168 | auto const initial_guess = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; 169 | MemeticIkTestParams params; 170 | params.memetic_params.num_threads = 4; 171 | params.print_debug = true; 172 | 173 | auto const maybe_solution = solve_memetic_ik_test(robot_model, 174 | "panda_arm", 175 | "panda_hand", 176 | goal_frame, 177 | initial_guess, 178 | params); 179 | 180 | REQUIRE(maybe_solution.has_value()); 181 | auto const final_frame = fk_fn(maybe_solution.value())[0]; 182 | CHECK(goal_frame.isApprox(final_frame, params.position_threshold)); 183 | } 184 | 185 | SECTION("Panda model IK, with joint centering and limits avoiding.") { 186 | auto const goal_frame = fk_fn(home_joint_angles)[0]; 187 | auto const initial_guess = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; 188 | 189 | MemeticIkTestParams params; 190 | params.center_joints_weight = 0.01; 191 | params.avoid_joint_limits_weight = 0.01; 192 | params.cost_threshold = 0.01; // Need to raise this for joint centering 193 | params.position_threshold = 0.01; 194 | params.print_debug = false; 195 | 196 | auto const maybe_solution = solve_memetic_ik_test(robot_model, 197 | "panda_arm", 198 | "panda_hand", 199 | goal_frame, 200 | initial_guess, 201 | params); 202 | 203 | REQUIRE(maybe_solution.has_value()); 204 | auto const final_frame = fk_fn(maybe_solution.value())[0]; 205 | CHECK(goal_frame.isApprox(final_frame, params.position_threshold)); 206 | } 207 | } 208 | -------------------------------------------------------------------------------- /tests/ik_tests.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | auto make_rr_model_for_ik() { 16 | /** 17 | * We aim to make an RR robot like this: 18 | * 19 | * revolute_2 --> (b) ==== |E <-- ee 20 | * // 21 | * // 22 | * (a) <-- revolute_1 23 | * origin 24 | **/ 25 | auto builder = moveit::core::RobotModelBuilder("rr", "base"); 26 | 27 | // Define transforms and joint axes 28 | geometry_msgs::msg::Pose origin; 29 | origin.orientation.w = 1.0; 30 | 31 | geometry_msgs::msg::Pose tform_x1; 32 | tform_x1.position.x = 1.0; 33 | tform_x1.orientation.w = 1.0; 34 | 35 | geometry_msgs::msg::Pose tform_x2; 36 | tform_x2.position.x = 2.0; 37 | tform_x2.orientation.w = 1.0; 38 | 39 | auto const z_axis = urdf::Vector3(0, 0, 1); 40 | 41 | // Build the actual robot chain. 42 | builder.addChain("base->a", "revolute", {origin}, z_axis); 43 | builder.addChain("a->b", "revolute", {tform_x2}, z_axis); 44 | builder.addChain("b->ee", "fixed", {tform_x1}); 45 | builder.addGroupChain("base", "ee", "group"); 46 | CHECK(builder.isValid()); 47 | return builder.build(); 48 | } 49 | 50 | TEST_CASE("RR model FK") { 51 | auto const robot_model = make_rr_model_for_ik(); 52 | 53 | auto const jmg = robot_model->getJointModelGroup("group"); 54 | auto const tip_link_indices = pick_ik::get_link_indices(robot_model, {"ee"}).value(); 55 | 56 | std::mutex mx; 57 | auto const fk_fn = pick_ik::make_fk_fn(robot_model, jmg, mx, tip_link_indices); 58 | 59 | SECTION("Zero joint position") { 60 | std::vector const joint_vals = {0.0, 0.0}; 61 | auto const result = fk_fn(joint_vals); 62 | CHECK(result[0].translation().x() == Catch::Approx(3.0)); 63 | CHECK(result[0].translation().y() == Catch::Approx(0.0)); 64 | } 65 | 66 | SECTION("Non-zero joint position") { 67 | std::vector const joint_vals = {M_PI_4, -M_PI_4}; 68 | auto const expected_x = 2.0 * std::cos(M_PI_4) + 1.0; 69 | auto const expected_y = 2.0 * std::sin(M_PI_4); 70 | 71 | auto const result = fk_fn(joint_vals); 72 | CHECK(result[0].translation().x() == Catch::Approx(expected_x).margin(0.001)); 73 | CHECK(result[0].translation().y() == Catch::Approx(expected_y).margin(0.001)); 74 | } 75 | } 76 | 77 | // Helper param struct and function to test IK solution. 78 | struct IkTestParams { 79 | double position_threshold = 0.0001; 80 | double orientation_threshold = 0.001; 81 | double cost_threshold = 0.0001; 82 | double position_scale = 1.0; 83 | double rotation_scale = 1.0; 84 | bool return_approximate_solution = false; 85 | pick_ik::GradientIkParams gd_params; 86 | }; 87 | 88 | auto solve_ik_test(moveit::core::RobotModelPtr robot_model, 89 | std::string const group_name, 90 | std::string const goal_frame_name, 91 | Eigen::Isometry3d const& goal_frame, 92 | std::vector const& initial_guess, 93 | IkTestParams const& params = IkTestParams()) 94 | -> std::optional> { 95 | // Make forward kinematics function 96 | auto const jmg = robot_model->getJointModelGroup(group_name); 97 | auto const tip_link_indices = pick_ik::get_link_indices(robot_model, {goal_frame_name}).value(); 98 | std::mutex mx; 99 | auto const fk_fn = pick_ik::make_fk_fn(robot_model, jmg, mx, tip_link_indices); 100 | 101 | // Make solution function 102 | auto const test_position = (params.position_scale > 0); 103 | std::optional position_threshold = std::nullopt; 104 | if (test_position) { 105 | position_threshold = params.position_threshold; 106 | } 107 | auto const test_rotation = (params.rotation_scale > 0.0); 108 | std::optional orientation_threshold = std::nullopt; 109 | if (test_rotation) { 110 | orientation_threshold = params.orientation_threshold; 111 | } 112 | auto const frame_tests = 113 | pick_ik::make_frame_tests({goal_frame}, position_threshold, orientation_threshold); 114 | auto const cost_function = 115 | kinematics::KinematicsBase::IKCostFn(); // What should be instantiated here? 116 | std::vector goals = {}; // TODO: Only works if empty. 117 | auto const solution_fn = 118 | pick_ik::make_is_solution_test_fn(frame_tests, goals, params.cost_threshold, fk_fn); 119 | 120 | // Make pose cost function 121 | auto const pose_cost_functions = pick_ik::make_pose_cost_functions({goal_frame}, 122 | params.position_scale, 123 | params.rotation_scale); 124 | CHECK(pose_cost_functions.size() == 1); 125 | 126 | // Solve IK 127 | auto const robot = pick_ik::Robot::from(robot_model, jmg, tip_link_indices); 128 | auto const cost_fn = pick_ik::make_cost_fn(pose_cost_functions, goals, fk_fn); 129 | return pick_ik::ik_gradient(initial_guess, 130 | robot, 131 | cost_fn, 132 | solution_fn, 133 | params.gd_params, 134 | params.return_approximate_solution); 135 | } 136 | 137 | TEST_CASE("RR model IK") { 138 | auto const robot_model = make_rr_model_for_ik(); 139 | 140 | SECTION("Zero joint angles with close initial guess") { 141 | Eigen::Isometry3d const goal_frame = 142 | Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity(); 143 | std::vector const expected_joint_angles = {0.0, 0.0}; 144 | std::vector const initial_guess = {0.1, -0.1}; 145 | 146 | auto const maybe_solution = 147 | solve_ik_test(robot_model, "group", "ee", goal_frame, initial_guess); 148 | 149 | REQUIRE(maybe_solution.has_value()); 150 | CHECK(maybe_solution.value()[0] == Catch::Approx(expected_joint_angles[0]).margin(0.01)); 151 | CHECK(maybe_solution.value()[1] == Catch::Approx(expected_joint_angles[1]).margin(0.01)); 152 | } 153 | 154 | SECTION("Zero joint angles with far initial guess") { 155 | Eigen::Isometry3d const goal_frame = 156 | Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity(); 157 | std::vector const expected_joint_angles = {0.0, 0.0}; 158 | std::vector const initial_guess = {M_PI_2, -M_PI_2}; 159 | 160 | auto const maybe_solution = 161 | solve_ik_test(robot_model, "group", "ee", goal_frame, initial_guess); 162 | 163 | REQUIRE(maybe_solution.has_value()); 164 | CHECK(maybe_solution.value()[0] == Catch::Approx(expected_joint_angles[0]).margin(0.01)); 165 | CHECK(maybe_solution.value()[1] == Catch::Approx(expected_joint_angles[1]).margin(0.01)); 166 | } 167 | 168 | SECTION("Nonzero joint angles with near initial guess") { 169 | Eigen::Isometry3d const goal_frame = 170 | Eigen::Translation3d(std::sin(M_PI_4), 3.0 * std::sin(M_PI_4), 0.0) * 171 | Eigen::AngleAxisd(0.75 * M_PI, Eigen::Vector3d::UnitZ()); 172 | std::vector const expected_joint_angles = {M_PI_4, M_PI_2}; 173 | std::vector const initial_guess = {M_PI_4 + 0.1, M_PI_2 - 0.1}; 174 | 175 | auto const maybe_solution = 176 | solve_ik_test(robot_model, "group", "ee", goal_frame, initial_guess); 177 | 178 | REQUIRE(maybe_solution.has_value()); 179 | CHECK(maybe_solution.value()[0] == Catch::Approx(expected_joint_angles[0]).margin(0.01)); 180 | CHECK(maybe_solution.value()[1] == Catch::Approx(expected_joint_angles[1]).margin(0.01)); 181 | } 182 | 183 | SECTION("Nonzero joint angles with far initial guess") { 184 | Eigen::Isometry3d const goal_frame = 185 | Eigen::Translation3d(std::sin(M_PI_4), 3.0 * std::sin(M_PI_4), 0.0) * 186 | Eigen::AngleAxisd(0.75 * M_PI, Eigen::Vector3d::UnitZ()); 187 | std::vector const expected_joint_angles = {M_PI_4, M_PI_2}; 188 | std::vector const initial_guess = {0.0, 0.0}; 189 | 190 | auto const maybe_solution = 191 | solve_ik_test(robot_model, "group", "ee", goal_frame, initial_guess); 192 | 193 | REQUIRE(maybe_solution.has_value()); 194 | CHECK(maybe_solution.value()[0] == Catch::Approx(expected_joint_angles[0]).margin(0.01)); 195 | CHECK(maybe_solution.value()[1] == Catch::Approx(expected_joint_angles[1]).margin(0.01)); 196 | } 197 | 198 | SECTION("Unreachable position") { 199 | auto const goal_frame = Eigen::Isometry3d::Identity(); 200 | std::vector const expected_joint_angles = {0.0, 0.0}; // Doesn't matter 201 | std::vector const initial_guess = {0.0, 0.0}; 202 | 203 | auto const maybe_solution = 204 | solve_ik_test(robot_model, "group", "ee", goal_frame, initial_guess); 205 | 206 | CHECK(!maybe_solution.has_value()); 207 | } 208 | 209 | SECTION("Reachable position, but not orientation") { 210 | Eigen::Isometry3d const goal_frame = 211 | Eigen::Translation3d(std::sin(M_PI_4), 3.0 * std::sin(M_PI_4), 0.0) * 212 | Eigen::Quaterniond::Identity(); 213 | std::vector const expected_joint_angles = {0.0, 0.0}; // Doesn't matter 214 | std::vector const initial_guess = {0.0, 0.0}; 215 | 216 | auto const maybe_solution = 217 | solve_ik_test(robot_model, "group", "ee", goal_frame, initial_guess); 218 | 219 | CHECK(!maybe_solution.has_value()); 220 | } 221 | 222 | SECTION("Reachable position, but not orientation -- zero rotation scale") { 223 | Eigen::Isometry3d const goal_frame = 224 | Eigen::Translation3d(std::sin(M_PI_4), 3.0 * std::sin(M_PI_4), 0.0) * 225 | Eigen::Quaterniond::Identity(); 226 | std::vector const expected_joint_angles = {M_PI_4, M_PI_2}; // Doesn't matter 227 | std::vector const initial_guess = {M_PI_4 + 0.1, M_PI_2 - 0.1}; 228 | auto params = IkTestParams(); 229 | params.rotation_scale = 0.0; 230 | 231 | auto const maybe_solution = 232 | solve_ik_test(robot_model, "group", "ee", goal_frame, initial_guess, params); 233 | 234 | CHECK(maybe_solution.has_value()); 235 | CHECK(maybe_solution.value()[0] == Catch::Approx(expected_joint_angles[0]).margin(0.01)); 236 | CHECK(maybe_solution.value()[1] == Catch::Approx(expected_joint_angles[1]).margin(0.01)); 237 | } 238 | } 239 | 240 | TEST_CASE("Panda model IK") { 241 | using moveit::core::loadTestingRobotModel; 242 | auto const robot_model = loadTestingRobotModel("panda"); 243 | 244 | auto const jmg = robot_model->getJointModelGroup("panda_arm"); 245 | auto const tip_link_indices = pick_ik::get_link_indices(robot_model, {"panda_hand"}).value(); 246 | std::mutex mx; 247 | auto const fk_fn = pick_ik::make_fk_fn(robot_model, jmg, mx, tip_link_indices); 248 | 249 | std::vector const home_joint_angles = 250 | {0.0, -M_PI_4, 0.0, -3.0 * M_PI_4, 0.0, M_PI_2, M_PI_4}; 251 | 252 | SECTION("Panda model IK at exact home values") { 253 | auto const goal_frame = fk_fn(home_joint_angles)[0]; 254 | auto const initial_guess = home_joint_angles; 255 | auto params = IkTestParams(); 256 | params.rotation_scale = 0.5; 257 | 258 | auto const maybe_solution = solve_ik_test(robot_model, 259 | "panda_arm", 260 | "panda_hand", 261 | goal_frame, 262 | initial_guess, 263 | params); 264 | 265 | REQUIRE(maybe_solution.has_value()); 266 | for (size_t i = 0; i < initial_guess.size(); ++i) { 267 | CHECK(maybe_solution.value()[i] == Catch::Approx(home_joint_angles[i]).margin(0.01)); 268 | } 269 | } 270 | 271 | SECTION("Panda model IK at perturbed home values") { 272 | std::vector const actual_joint_angles = 273 | {0.1, -M_PI_4 - 0.1, 0.1, -3.0 * M_PI_4 - 0.1, 0.1, M_PI_2 - 0.1, M_PI_4 + 0.1}; 274 | auto const goal_frame = fk_fn(actual_joint_angles)[0]; 275 | 276 | auto const initial_guess = home_joint_angles; 277 | auto params = IkTestParams(); 278 | params.rotation_scale = 0.5; 279 | 280 | auto const maybe_solution = solve_ik_test(robot_model, 281 | "panda_arm", 282 | "panda_hand", 283 | goal_frame, 284 | initial_guess, 285 | params); 286 | 287 | REQUIRE(maybe_solution.has_value()); 288 | for (size_t i = 0; i < initial_guess.size(); ++i) { 289 | // Note the extra tolerance... 290 | CHECK(maybe_solution.value()[i] == Catch::Approx(actual_joint_angles[i]).margin(0.025)); 291 | } 292 | } 293 | } 294 | -------------------------------------------------------------------------------- /tests/robot_tests.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | auto make_rr_model() { 10 | /** 11 | * We aim to make an RR robot like this: 12 | * 13 | * revolute_2 --> (b) ==== |E <-- ee 14 | * // 15 | * // 16 | * (a) <-- revolute_1 17 | * origin 18 | **/ 19 | auto builder = moveit::core::RobotModelBuilder("rr", "base"); 20 | 21 | // Define transforms and joint axes 22 | geometry_msgs::msg::Pose origin; 23 | origin.orientation.w = 1.0; 24 | 25 | geometry_msgs::msg::Pose tform_x1; 26 | tform_x1.position.x = 1.0; // This is the length of each link. 27 | tform_x1.orientation.w = 1.0; 28 | 29 | auto const z_axis = urdf::Vector3(0, 0, 1); 30 | 31 | // Build the actual robot chain. 32 | builder.addChain("base->a", "revolute", {origin}, z_axis); 33 | builder.addChain("a->b", "revolute", {tform_x1}, z_axis); 34 | builder.addChain("b->ee", "fixed", {tform_x1}); 35 | builder.addGroupChain("base", "ee", "group"); 36 | CHECK(builder.isValid()); 37 | return builder.build(); 38 | } 39 | 40 | TEST_CASE("pick_ik::get_link_indices") { 41 | auto const robot_model = make_rr_model(); 42 | 43 | SECTION("second link") { 44 | auto const tip_link_indices = pick_ik::get_link_indices(robot_model, {"b"}); 45 | REQUIRE(tip_link_indices.has_value()); 46 | CHECK(tip_link_indices->size() == 1); 47 | CHECK(tip_link_indices->at(0) == 2); 48 | } 49 | 50 | SECTION("end effector link") { 51 | auto const tip_link_indices = pick_ik::get_link_indices(robot_model, {"ee"}); 52 | REQUIRE(tip_link_indices.has_value()); 53 | CHECK(tip_link_indices->size() == 1); 54 | CHECK(tip_link_indices->at(0) == 3); 55 | } 56 | 57 | SECTION("multiple links") { 58 | auto const tip_link_indices = pick_ik::get_link_indices(robot_model, {"a", "b", "ee"}); 59 | REQUIRE(tip_link_indices.has_value()); 60 | CHECK(tip_link_indices->size() == 3); 61 | CHECK(tip_link_indices->at(0) == 1); 62 | CHECK(tip_link_indices->at(1) == 2); 63 | CHECK(tip_link_indices->at(2) == 3); 64 | } 65 | 66 | SECTION("no joints") { 67 | auto const tip_link_indices = pick_ik::get_link_indices(robot_model, {}); 68 | REQUIRE(tip_link_indices.has_value()); 69 | CHECK(tip_link_indices->size() == 0); 70 | } 71 | 72 | SECTION("invalid joint") { 73 | auto const tip_link_indices = pick_ik::get_link_indices(robot_model, {"c"}); 74 | REQUIRE(tip_link_indices.has_value() == false); 75 | } 76 | 77 | SECTION("base joint") { 78 | auto const tip_link_indices = pick_ik::get_link_indices(robot_model, {"base"}); 79 | REQUIRE(tip_link_indices.has_value()); 80 | CHECK(tip_link_indices->size() == 1); 81 | CHECK(tip_link_indices->at(0) == 0); 82 | } 83 | } 84 | 85 | TEST_CASE("pick_ik::Robot::from -- Simple RR Model") { 86 | auto const robot_model = make_rr_model(); 87 | auto* const jmg = robot_model->getJointModelGroup("group"); 88 | auto const tip_link_indices = 89 | pick_ik::get_link_indices(robot_model, {"ee"}) 90 | .or_else([](auto const& error) { throw std::invalid_argument(error); }) 91 | .value(); 92 | auto const robot = pick_ik::Robot::from(robot_model, jmg, tip_link_indices); 93 | 94 | SECTION("RR robot has two joints") { CHECK(robot.variables.size() == 2); } 95 | } 96 | 97 | TEST_CASE("pick_ik::Robot::from -- Panda Model") { 98 | using moveit::core::loadTestingRobotModel; 99 | auto const robot_model = loadTestingRobotModel("panda"); 100 | auto* const jmg = robot_model->getJointModelGroup("panda_arm"); 101 | auto const tip_link_indices = 102 | pick_ik::get_link_indices(robot_model, {"panda_hand"}) 103 | .or_else([](auto const& error) { throw std::invalid_argument(error); }) 104 | .value(); 105 | auto const robot = pick_ik::Robot::from(robot_model, jmg, tip_link_indices); 106 | 107 | SECTION("Panda has seven joints") { CHECK(robot.variables.size() == 7); } 108 | } 109 | --------------------------------------------------------------------------------