├── .ci.rosinstall ├── .clang-format ├── .github └── workflows │ ├── ci_bionic.yml │ └── ci_focal.yml ├── .gitignore ├── .gitmodules ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include └── moveit_opw_kinematics_plugin │ └── moveit_opw_kinematics_plugin.h ├── moveit_opw_kinematics_plugin_description.xml ├── package.xml ├── src └── moveit_opw_kinematics_plugin.cpp └── test ├── faulty_kinematics_fanuc.yaml ├── faulty_kinematics_kuka.yaml ├── kinematics_fanuc.yaml ├── test_fanuc.cpp ├── test_fanuc.rostest ├── test_fanuc_faulty_params.rostest ├── test_faulty_params.cpp ├── test_kuka.rostest ├── test_kuka_faulty_params.rostest ├── test_kuka_specific.cpp ├── test_plugin.cpp └── test_utils.h /.ci.rosinstall: -------------------------------------------------------------------------------- 1 | # test dependencies 2 | - git: {local-name : kuka_test_resources, uri: 'https://github.com/JeroenDM/kuka_test_resources.git', version: master} -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Google 2 | AccessModifierOffset: -2 3 | ConstructorInitializerIndentWidth: 2 4 | AlignEscapedNewlinesLeft: false 5 | AlignTrailingComments: true 6 | AllowAllParametersOfDeclarationOnNextLine: false 7 | AllowShortIfStatementsOnASingleLine: false 8 | AllowShortLoopsOnASingleLine: false 9 | AllowShortFunctionsOnASingleLine: None 10 | AllowShortLoopsOnASingleLine: false 11 | AlwaysBreakTemplateDeclarations: true 12 | AlwaysBreakBeforeMultilineStrings: false 13 | BreakBeforeBinaryOperators: false 14 | BreakBeforeTernaryOperators: false 15 | BreakConstructorInitializersBeforeComma: true 16 | BinPackParameters: true 17 | ColumnLimit: 120 18 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 19 | DerivePointerBinding: false 20 | PointerBindsToType: true 21 | ExperimentalAutoDetectBinPacking: false 22 | IndentCaseLabels: true 23 | MaxEmptyLinesToKeep: 1 24 | NamespaceIndentation: None 25 | ObjCSpaceBeforeProtocolList: true 26 | PenaltyBreakBeforeFirstCallParameter: 19 27 | PenaltyBreakComment: 60 28 | PenaltyBreakString: 1 29 | PenaltyBreakFirstLessLess: 1000 30 | PenaltyExcessCharacter: 1000 31 | PenaltyReturnTypeOnItsOwnLine: 90 32 | SpacesBeforeTrailingComments: 2 33 | Cpp11BracedListStyle: false 34 | Standard: Auto 35 | IndentWidth: 2 36 | TabWidth: 2 37 | UseTab: Never 38 | IndentFunctionDeclarationAfterType: false 39 | SpacesInParentheses: false 40 | SpacesInAngles: false 41 | SpaceInEmptyParentheses: false 42 | SpacesInCStyleCastParentheses: false 43 | SpaceAfterControlStatementKeyword: true 44 | SpaceBeforeAssignmentOperators: true 45 | ContinuationIndentWidth: 4 46 | SortIncludes: false 47 | SpaceAfterCStyleCast: false 48 | 49 | # Configure each individual brace in BraceWrapping 50 | BreakBeforeBraces: Custom 51 | 52 | # Control of individual brace wrapping cases 53 | BraceWrapping: { 54 | AfterClass: 'true' 55 | AfterControlStatement: 'true' 56 | AfterEnum : 'true' 57 | AfterFunction : 'true' 58 | AfterNamespace : 'true' 59 | AfterStruct : 'true' 60 | AfterUnion : 'true' 61 | BeforeCatch : 'true' 62 | BeforeElse : 'true' 63 | IndentBraces : 'false' 64 | } -------------------------------------------------------------------------------- /.github/workflows/ci_bionic.yml: -------------------------------------------------------------------------------- 1 | name: CI - Ubuntu Bionic 2 | 3 | on: 4 | # direct pushes to protected branches are not supported 5 | pull_request: 6 | # run every day, at 6am UTC 7 | schedule: 8 | - cron: '0 6 * * *' 9 | # allow manually starting this workflow 10 | workflow_dispatch: 11 | 12 | jobs: 13 | industrial_ci: 14 | name: ROS Melodic (${{ matrix.ros_repo }}) 15 | runs-on: ubuntu-18.04 16 | 17 | strategy: 18 | fail-fast: false 19 | matrix: 20 | ros_distro: [ melodic ] 21 | ros_repo: [ main, testing ] 22 | 23 | env: 24 | CCACHE_DIR: "${{ github.workspace }}/.ccache" 25 | CATKIN_LINT: true 26 | 27 | steps: 28 | - name: Fetch repository 29 | uses: actions/checkout@v2 30 | 31 | - name: ccache cache 32 | uses: actions/cache@v2 33 | with: 34 | path: ${{ env.CCACHE_DIR }} 35 | # we always want the ccache cache to be persisted, as we cannot easily 36 | # determine whether dependencies have changed, and ccache will manage 37 | # updating the cache for us. Adding 'run_id' to the key will force an 38 | # upload at the end of the job. 39 | key: ccache-${{ matrix.ros_distro }}-${{ matrix.ros_repo }}-${{github.run_id}} 40 | restore-keys: | 41 | ccache-${{ matrix.ros_distro }}-${{ matrix.ros_repo }} 42 | 43 | - name: Run industrial_ci 44 | uses: ros-industrial/industrial_ci@master 45 | env: 46 | ROS_DISTRO: ${{ matrix.ros_distro }} 47 | ROS_REPO: ${{ matrix.ros_repo }} 48 | UPSTREAM_WORKSPACE: '.ci.rosinstall' 49 | -------------------------------------------------------------------------------- /.github/workflows/ci_focal.yml: -------------------------------------------------------------------------------- 1 | name: CI - Ubuntu Focal 2 | 3 | on: 4 | # direct pushes to protected branches are not supported 5 | pull_request: 6 | # run every day, at 6am UTC 7 | schedule: 8 | - cron: '0 6 * * *' 9 | # allow manually starting this workflow 10 | workflow_dispatch: 11 | 12 | jobs: 13 | industrial_ci: 14 | name: ROS Noetic (${{ matrix.ros_repo }}) 15 | runs-on: ubuntu-20.04 16 | 17 | strategy: 18 | fail-fast: false 19 | matrix: 20 | ros_distro: [ noetic ] 21 | ros_repo: [ main, testing ] 22 | 23 | env: 24 | CCACHE_DIR: "${{ github.workspace }}/.ccache" 25 | CATKIN_LINT: true 26 | 27 | steps: 28 | - name: Fetch repository 29 | uses: actions/checkout@v2 30 | 31 | - name: ccache cache 32 | uses: actions/cache@v2 33 | with: 34 | path: ${{ env.CCACHE_DIR }} 35 | # we always want the ccache cache to be persisted, as we cannot easily 36 | # determine whether dependencies have changed, and ccache will manage 37 | # updating the cache for us. Adding 'run_id' to the key will force an 38 | # upload at the end of the job. 39 | key: ccache-${{ matrix.ros_distro }}-${{ matrix.ros_repo }}-${{github.run_id}} 40 | restore-keys: | 41 | ccache-${{ matrix.ros_distro }}-${{ matrix.ros_repo }} 42 | 43 | - name: Run industrial_ci 44 | uses: ros-industrial/industrial_ci@master 45 | env: 46 | ROS_DISTRO: ${{ matrix.ros_distro }} 47 | ROS_REPO: ${{ matrix.ros_repo }} 48 | UPSTREAM_WORKSPACE: '.ci.rosinstall' 49 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JeroenDM/moveit_opw_kinematics_plugin/fa51c00b356333d9f628c0e5a87520e26abc622d/.gitmodules -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package moveit_opw_kinematics_plugin 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.4.0 (2022-01-05) 6 | ------------------ 7 | * Update status badge noetic-devel (default) branch 8 | * Run melodic ci job on ubuntu 18.04 9 | * Sync melodic changes to noetic 10 | - fix bug introduced when cherry-picking commits 11 | - minor formatting improvement 12 | - replace logstring with variable LOGNAME 13 | - print out odd posture used in self-test on error 14 | - Print all deviations and check zero pose as well 15 | * Update Jeroen's email address 16 | * Add github actions 17 | - Add roscpp as CATKIN_DEPENDS 18 | - Use the .ci.rosinstall to install dependencies 19 | - Move rosinstall for testing to typical place for industrial-ci 20 | - Update ci badges to GHA 21 | - Remove travis 22 | * Contributors: JeroenDM, Simon Schmeisser 23 | 24 | 0.3.1 (2021-09-22) 25 | ------------------ 26 | * First release in noetic. 27 | * update to new opw_kinematics API (with std::array) 28 | * Add opw_kinematics dependency to package.xml and CMakeLists.txt. 29 | * Remove opw_kinematics submodule. 30 | * Change moveit_resources to more specific moveit_resources_fanuc_moveit_config 31 | * Minor changes: 32 | - It's not a service based IK solver. 33 | - Parameters don't always come from 'kinematics.yaml'. 34 | * Contributors: JeroenDM, gavanderhoorn. 35 | 36 | 0.2.1 (2020-05-13) 37 | ------------------ 38 | * First release in melodic. 39 | 40 | 0.1.1 (2020-05-13) 41 | ------------------ 42 | * First release in kinetic. -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(moveit_opw_kinematics_plugin) 3 | 4 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 5 | set(CMAKE_CXX_EXTENSIONS OFF) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | eigen_conversions 12 | moveit_core 13 | roscpp 14 | pluginlib 15 | ) 16 | 17 | set(MOVEIT_LIB_NAME moveit_opw_kinematics_plugin) 18 | 19 | # System dependencies are found with CMake's conventions 20 | find_package(Boost REQUIRED COMPONENTS filesystem) 21 | 22 | find_package(opw_kinematics REQUIRED) 23 | 24 | ################################### 25 | ## catkin specific configuration ## 26 | ################################### 27 | ## The catkin_package macro generates cmake config files for your package 28 | ## Declare things to be passed to dependent projects 29 | ## INCLUDE_DIRS: uncomment this if your package contains header files 30 | ## LIBRARIES: libraries you create in this project that dependent projects also need 31 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 32 | ## DEPENDS: system dependencies of this project that dependent projects also need 33 | catkin_package( 34 | LIBRARIES ${MOVEIT_LIB_NAME} 35 | CATKIN_DEPENDS roscpp 36 | ) 37 | 38 | ########### 39 | ## Build ## 40 | ########### 41 | 42 | 43 | ## Specify additional locations of header files 44 | ## Your package locations should be listed before other locations 45 | include_directories( 46 | include 47 | ${catkin_INCLUDE_DIRS} 48 | ) 49 | 50 | # Declare a C++ library 51 | add_library(${MOVEIT_LIB_NAME} 52 | src/moveit_opw_kinematics_plugin.cpp 53 | ) 54 | 55 | target_link_libraries(${MOVEIT_LIB_NAME} 56 | ${catkin_LIBRARIES} 57 | opw_kinematics::opw_kinematics 58 | ) 59 | 60 | ############# 61 | ## Install ## 62 | ############# 63 | 64 | # Mark executables and/or libraries for installation 65 | install(TARGETS ${MOVEIT_LIB_NAME} 66 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 67 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 68 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 69 | ) 70 | 71 | install( 72 | FILES 73 | moveit_opw_kinematics_plugin_description.xml 74 | DESTINATION 75 | ${CATKIN_PACKAGE_SHARE_DESTINATION}) 76 | 77 | ############# 78 | ## Testing ## 79 | ############# 80 | 81 | if(CATKIN_ENABLE_TESTING) 82 | find_package(moveit_ros_planning REQUIRED) 83 | find_package(rostest REQUIRED) 84 | 85 | include_directories(test ${moveit_ros_planning_INCLUDE_DIRS}) 86 | 87 | add_rostest_gtest(${PROJECT_NAME}_test_kuka 88 | test/test_kuka.rostest test/test_plugin.cpp 89 | test/test_kuka_specific.cpp 90 | ) 91 | target_link_libraries(${PROJECT_NAME}_test_kuka 92 | gtest 93 | ${moveit_ros_planning_LIBRARIES} 94 | ${catkin_LIBRARIES} 95 | ${MOVEIT_LIB_NAME} 96 | ) 97 | 98 | add_rostest_gtest(${PROJECT_NAME}_test_fanuc 99 | test/test_fanuc.rostest 100 | test/test_plugin.cpp 101 | ) 102 | target_link_libraries(${PROJECT_NAME}_test_fanuc 103 | gtest 104 | ${moveit_ros_planning_LIBRARIES} 105 | ${catkin_LIBRARIES} 106 | ${MOVEIT_LIB_NAME} 107 | ) 108 | 109 | add_rostest_gtest(${PROJECT_NAME}_test_fanuc_faulty 110 | test/test_fanuc_faulty_params.rostest 111 | test/test_faulty_params.cpp 112 | ) 113 | target_link_libraries(${PROJECT_NAME}_test_fanuc_faulty 114 | gtest 115 | ${moveit_ros_planning_LIBRARIES} 116 | ${catkin_LIBRARIES} 117 | ${MOVEIT_LIB_NAME} 118 | ) 119 | 120 | add_rostest_gtest(${PROJECT_NAME}_test_kuka_faulty 121 | test/test_kuka_faulty_params.rostest 122 | test/test_faulty_params.cpp 123 | ) 124 | target_link_libraries(${PROJECT_NAME}_test_kuka_faulty 125 | gtest 126 | ${moveit_ros_planning_LIBRARIES} 127 | ${catkin_LIBRARIES} 128 | ${MOVEIT_LIB_NAME} 129 | ) 130 | endif() 131 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # opw_kinematics_plugin 2 | 3 | [![Build status Ubuntu Bionic](https://github.com/JeroenDM/moveit_opw_kinematics_plugin/actions/workflows/ci_bionic.yml/badge.svg?branch=noetic-devel)](https://github.com/JeroenDM/moveit_opw_kinematics_plugin/actions/workflows/ci_bionic.yml) 4 | [![Build status Ubuntu Focal](https://github.com/JeroenDM/moveit_opw_kinematics_plugin/actions/workflows/ci_focal.yml/badge.svg?branch=noetic-devel)](https://github.com/JeroenDM/moveit_opw_kinematics_plugin/actions/workflows/ci_focal.yml) 5 | 6 | An attempt at writing a [MoveIt!](https://moveit.ros.org/) plugin for [opw_kinematics](https://github.com/Jmeyer1292/opw_kinematics). The package calculates closed form inverse kinematic solutions for typical industrial robots and was created by [Jmeyer1292](https://github.com/Jmeyer1292). A MoveIt! plugin for this package could be an alternative for the [ikfast plugin](https://github.com/ros-planning/moveit/tree/kinetic-devel/moveit_kinematics/ikfast_kinematics_plugin). 7 | 8 | See [issues](https://github.com/JeroenDM/moveit_opw_kinematics_plugin/issues) the follow the implementation process. (Where I mostly have conversations with myself.) 9 | 10 | The general template is copied from the moveit [srv_kinematics_plugin](https://github.com/ros-planning/moveit/tree/kinetic-devel/moveit_kinematics/srv_kinematics_plugin). 11 | Some functions are directly copied from the package [descartes_opw_model](https://github.com/Jmeyer1292/descartes_opw_model). 12 | 13 | To use this plugin with another robot, clone this package inside your workspace: 14 | ```bash 15 | cd catkin_ws/src/ 16 | git clone --recursive https://github.com/JeroenDM/moveit_opw_kinematics_plugin.git 17 | ``` 18 | 19 | And also add a [moveit configuration](http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/setup_assistant/setup_assistant_tutorial.html) for a compatible robot. You have to update the config/kinematics.yaml file. It looks like this for a Kuka kr6r700: 20 | 21 | ```yaml 22 | manipulator: 23 | kinematics_solver: moveit_opw_kinematics_plugin/MoveItOPWKinematicsPlugin 24 | opw_kinematics_geometric_parameters: 25 | a1: 0.025 26 | a2: -0.035 27 | b: 0.000 28 | c1: 0.400 29 | c2: 0.315 30 | c3: 0.365 31 | c4: 0.080 32 | opw_kinematics_joint_offsets: [0.0, -1.57079632679, 0, 0, 0, 0] 33 | opw_kinematics_joint_sign_corrections: [-1, 1, 1, -1, 1, -1] 34 | ``` 35 | -------------------------------------------------------------------------------- /include/moveit_opw_kinematics_plugin/moveit_opw_kinematics_plugin.h: -------------------------------------------------------------------------------- 1 | #ifndef MOVEIT_OPW_KINEMATICS_PLUGIN_ 2 | #define MOVEIT_OPW_KINEMATICS_PLUGIN_ 3 | 4 | // ROS 5 | #include 6 | 7 | // System 8 | #include 9 | 10 | // ROS msgs 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | // MoveIt! 18 | #include 19 | #include 20 | #include 21 | 22 | // OPW kinematics 23 | #include "opw_kinematics/opw_kinematics.h" 24 | 25 | namespace moveit_opw_kinematics_plugin 26 | { 27 | using kinematics::KinematicsResult; 28 | /** 29 | * @brief Specific implementation of kinematics using ROS service calls to communicate with 30 | external IK solvers. This version can be used with any robot. Supports non-chain kinematic groups 31 | */ 32 | class MoveItOPWKinematicsPlugin : public kinematics::KinematicsBase 33 | { 34 | public: 35 | // struct for storing and sorting solutions 36 | struct LimitObeyingSol 37 | { 38 | std::vector value; 39 | double dist_from_seed; 40 | 41 | bool operator<(const LimitObeyingSol& a) const 42 | { 43 | return dist_from_seed < a.dist_from_seed; 44 | } 45 | }; 46 | 47 | /** 48 | * @brief Default constructor 49 | */ 50 | MoveItOPWKinematicsPlugin(); 51 | 52 | virtual bool 53 | getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, 54 | std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, 55 | const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; 56 | 57 | virtual bool 58 | searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, 59 | std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, 60 | const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; 61 | 62 | virtual bool 63 | searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, 64 | const std::vector& consistency_limits, std::vector& solution, 65 | moveit_msgs::MoveItErrorCodes& error_code, 66 | const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; 67 | 68 | virtual bool searchPositionIK( 69 | const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, 70 | std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code, 71 | const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; 72 | 73 | virtual bool 74 | searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, 75 | const std::vector& consistency_limits, std::vector& solution, 76 | const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code, 77 | const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; 78 | 79 | virtual bool getPositionFK(const std::vector& link_names, const std::vector& joint_angles, 80 | std::vector& poses) const override; 81 | 82 | virtual bool initialize(const moveit::core::RobotModel& robot_model, const std::string& group_name, 83 | const std::string& base_frame, const std::vector& tip_frames, 84 | double search_discretization) override; 85 | 86 | /** 87 | * @brief Return all the joint names in the order they are used internally 88 | */ 89 | const std::vector& getJointNames() const override; 90 | 91 | /** 92 | * @brief Return all the link names in the order they are represented internally 93 | */ 94 | const std::vector& getLinkNames() const override; 95 | 96 | /** 97 | * @brief Return all the variable names in the order they are represented internally 98 | */ 99 | const std::vector& getVariableNames() const; 100 | 101 | virtual bool 102 | getPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, 103 | std::vector>& solutions, KinematicsResult& result, 104 | const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; 105 | 106 | protected: 107 | virtual bool 108 | searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, 109 | std::vector& solution, const IKCallbackFn& solution_callback, 110 | moveit_msgs::MoveItErrorCodes& error_code, const std::vector& consistency_limits, 111 | const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; 112 | 113 | virtual bool 114 | searchPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, 115 | double timeout, const std::vector& consistency_limits, std::vector& solution, 116 | const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code, 117 | const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; 118 | 119 | virtual bool setRedundantJoints(const std::vector& redundant_joint_indices) override; 120 | 121 | private: 122 | bool timedOut(const ros::WallTime& start_time, double duration) const; 123 | 124 | int getJointIndex(const std::string& name) const; 125 | 126 | bool isRedundantJoint(unsigned int index) const; 127 | 128 | bool setOPWParameters(); 129 | 130 | static double distance(const std::vector& a, const std::vector& b); 131 | static std::size_t closestJointPose(const std::vector& target, 132 | const std::vector>& candidates); 133 | bool getAllIK(const Eigen::Isometry3d& pose, std::vector>& joint_poses) const; 134 | bool getIK(const Eigen::Isometry3d& pose, const std::vector& seed_state, 135 | std::vector& joint_pose) const; 136 | 137 | /** 138 | * @brief append IK solutions by adding +-2pi 139 | * 140 | * For all solutions, check if solution +-360° is still inside limits 141 | * An opw solution might be outside the joint limits, while the extended one is inside (e.g. asymmetric limits) 142 | * therefore this just extends the solution space, need to apply joint limits separately 143 | */ 144 | void expandIKSolutions(std::vector>& solutions) const; 145 | 146 | /** 147 | * @brief check forward and inverse kinematics consistency 148 | * 149 | * A basic tests to check if the geometric parameters loaded at initialization are correct. 150 | */ 151 | bool selfTest(const std::vector& joint_angles); 152 | 153 | /** 154 | * @brief check if two poses are the same within an absolute tolerance of 1e-6 155 | */ 156 | bool comparePoses(Eigen::Isometry3d& Ta, Eigen::Isometry3d& Tb); 157 | 158 | bool active_; /** Internal variable that indicates whether solvers are configured and ready */ 159 | 160 | moveit_msgs::KinematicSolverInfo ik_group_info_; /** Stores information for the inverse kinematics solver */ 161 | 162 | unsigned int dimension_; /** Dimension of the group */ 163 | 164 | const robot_model::JointModelGroup* joint_model_group_; 165 | 166 | int num_possible_redundant_joints_; 167 | 168 | opw_kinematics::Parameters opw_parameters_; 169 | }; 170 | } // namespace moveit_opw_kinematics_plugin 171 | 172 | #endif 173 | -------------------------------------------------------------------------------- /moveit_opw_kinematics_plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Analytic kinematics for industrial robots 5 | based on package opw_kinematics. 6 | 7 | 8 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | moveit_opw_kinematics_plugin 4 | 0.4.0 5 | 6 |

7 | MoveIt kinematics plugin for industrial robots. 8 |

9 |

10 | This plugin uses an analytical inverse kinematic library, opw_kinematics, 11 | to calculate the inverse kinematics for industrial robots with 6 degrees of freedom, 12 | two parallel joints, and a spherical wrist. 13 |

14 | 15 | 16 | Jeroen De Maeyer 17 | 18 | Apache2.0 19 | 20 | 21 | 22 | Jeroen De Maeyer 23 | Simon Schmeisser (isys vision) 24 | 25 | catkin 26 | 27 | eigen_conversions 28 | moveit_core 29 | roscpp 30 | pluginlib 31 | opw_kinematics 32 | 33 | moveit_core 34 | roscpp 35 | 36 | moveit_core 37 | roscpp 38 | pluginlib 39 | 40 | rostest 41 | moveit_resources_fanuc_moveit_config 42 | moveit_ros_planning 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /src/moveit_opw_kinematics_plugin.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | // abs 8 | #include 9 | #include // copy 10 | 11 | // Eigen 12 | #include 13 | #include 14 | #include 15 | 16 | // OPW kinematics 17 | #include "opw_kinematics/opw_io.h" 18 | #include "opw_kinematics/opw_kinematics.h" 19 | #include "opw_kinematics/opw_utilities.h" 20 | 21 | // register OPWKinematics as a KinematicsBase implementation 22 | CLASS_LOADER_REGISTER_CLASS(moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin, kinematics::KinematicsBase) 23 | 24 | namespace moveit_opw_kinematics_plugin 25 | { 26 | constexpr char LOGNAME[] = "opw"; 27 | using kinematics::KinematicsResult; 28 | 29 | MoveItOPWKinematicsPlugin::MoveItOPWKinematicsPlugin() : active_(false) 30 | { 31 | } 32 | 33 | bool MoveItOPWKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model, const std::string& group_name, 34 | const std::string& base_frame, const std::vector& tip_frames, 35 | double search_discretization) 36 | { 37 | bool debug = false; 38 | 39 | ROS_INFO_STREAM_NAMED(LOGNAME, "MoveItOPWKinematicsPlugin initializing"); 40 | 41 | storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization); 42 | 43 | joint_model_group_ = robot_model_->getJointModelGroup(group_name); 44 | if (!joint_model_group_) 45 | return false; 46 | 47 | if (debug) 48 | { 49 | std::cout << std::endl 50 | << "Joint Model Variable Names: " 51 | "------------------------------------------- " 52 | << std::endl; 53 | const std::vector jm_names = joint_model_group_->getVariableNames(); 54 | std::copy(jm_names.begin(), jm_names.end(), std::ostream_iterator(std::cout, "\n")); 55 | std::cout << std::endl; 56 | } 57 | 58 | // Get the dimension of the planning group 59 | dimension_ = joint_model_group_->getVariableCount(); 60 | ROS_INFO_STREAM_NAMED(LOGNAME, "Dimension planning group '" 61 | << group_name << "': " << dimension_ 62 | << ". Active Joints Models: " << joint_model_group_->getActiveJointModels().size() 63 | << ". Mimic Joint Models: " << joint_model_group_->getMimicJointModels().size()); 64 | 65 | // Copy joint names 66 | for (std::size_t i = 0; i < joint_model_group_->getActiveJointModels().size(); ++i) 67 | { 68 | ik_group_info_.joint_names.push_back(joint_model_group_->getActiveJointModelNames()[i]); 69 | } 70 | 71 | if (debug) 72 | { 73 | ROS_ERROR_STREAM_NAMED(LOGNAME, "tip links available:"); 74 | std::copy(tip_frames_.begin(), tip_frames_.end(), std::ostream_iterator(std::cout, "\n")); 75 | } 76 | 77 | // Make sure all the tip links are in the link_names vector 78 | for (std::size_t i = 0; i < tip_frames_.size(); ++i) 79 | { 80 | if (!joint_model_group_->hasLinkModel(tip_frames_[i])) 81 | { 82 | ROS_ERROR_NAMED(LOGNAME, "Could not find tip name '%s' in joint group '%s'", tip_frames_[i].c_str(), 83 | group_name.c_str()); 84 | return false; 85 | } 86 | ik_group_info_.link_names.push_back(tip_frames_[i]); 87 | } 88 | 89 | // set geometric parameters for opw model 90 | if (!setOPWParameters()) 91 | { 92 | ROS_ERROR_STREAM_NAMED(LOGNAME, "Could not load OPW parameters. Please make " 93 | "sure they are loaded on the parameter server and are of the correct type(s)."); 94 | return false; 95 | } 96 | 97 | // check geometric parameters for opw model 98 | if (!selfTest({ 0, 0, 0, 0, 0, 0 })) 99 | { 100 | ROS_ERROR_STREAM_NAMED(LOGNAME, "The OPW parameters loaded from the parameter " 101 | "server appear to be incorrect (self-test failed for '0 0 0 0 0 0')."); 102 | return false; 103 | } 104 | const std::vector joint_angles = { 0.1, -0.1, 0.2, -0.3, 0.5, -0.8 }; 105 | if (!selfTest(joint_angles)) 106 | { 107 | ROS_ERROR_STREAM_NAMED(LOGNAME, "The OPW parameters loaded from the parameter server appear to be incorrect" 108 | " (self-test failed for odd posture: '0.1, -0.1, 0.2, -0.3, 0.5, -0.8')."); 109 | return false; 110 | } 111 | 112 | active_ = true; 113 | ROS_DEBUG_NAMED(LOGNAME, "OPW kinematics solver initialized"); 114 | return true; 115 | } 116 | 117 | bool MoveItOPWKinematicsPlugin::setRedundantJoints(const std::vector& redundant_joints) 118 | { 119 | if (num_possible_redundant_joints_ < 0) 120 | { 121 | ROS_ERROR_NAMED(LOGNAME, "This group cannot have redundant joints"); 122 | return false; 123 | } 124 | if (redundant_joints.size() > static_cast(num_possible_redundant_joints_)) 125 | { 126 | ROS_ERROR_NAMED(LOGNAME, "This group can only have %d redundant joints", num_possible_redundant_joints_); 127 | return false; 128 | } 129 | 130 | redundant_joint_indices_ = redundant_joints; 131 | 132 | return true; 133 | } 134 | 135 | bool MoveItOPWKinematicsPlugin::isRedundantJoint(unsigned int index) const 136 | { 137 | for (std::size_t j = 0; j < redundant_joint_indices_.size(); ++j) 138 | if (redundant_joint_indices_[j] == index) 139 | return true; 140 | return false; 141 | } 142 | 143 | int MoveItOPWKinematicsPlugin::getJointIndex(const std::string& name) const 144 | { 145 | for (unsigned int i = 0; i < ik_group_info_.joint_names.size(); i++) 146 | { 147 | if (ik_group_info_.joint_names[i] == name) 148 | return i; 149 | } 150 | return -1; 151 | } 152 | 153 | bool MoveItOPWKinematicsPlugin::timedOut(const ros::WallTime& start_time, double duration) const 154 | { 155 | return ((ros::WallTime::now() - start_time).toSec() >= duration); 156 | } 157 | 158 | bool MoveItOPWKinematicsPlugin::selfTest(const std::vector& joint_angles) 159 | { 160 | // MoveIt takes std::vector, opw kinematics takes std::array 161 | std::array joint_angles_arr{}; 162 | std::copy_n(joint_angles.begin(), joint_angles_arr.size(), joint_angles_arr.begin()); 163 | 164 | auto fk_pose_opw = opw_kinematics::forward(opw_parameters_, joint_angles_arr); 165 | robot_state::RobotState robot_state(robot_model_); 166 | robot_state.setJointGroupPositions(joint_model_group_, joint_angles); 167 | auto fk_pose_moveit = robot_state.getGlobalLinkTransform(tip_frames_[0]); 168 | // group/robot might not be at origin, subtract base transform 169 | auto base = robot_state.getGlobalLinkTransform(base_frame_); 170 | fk_pose_moveit = base.inverse() * fk_pose_moveit; 171 | 172 | if (!comparePoses(fk_pose_opw, fk_pose_moveit)) 173 | { 174 | return false; 175 | } 176 | 177 | return true; 178 | } 179 | 180 | bool MoveItOPWKinematicsPlugin::comparePoses(Eigen::Isometry3d& Ta, Eigen::Isometry3d& Tb) 181 | { 182 | const float TOLERANCE = 1e-6; 183 | 184 | auto Ra = Ta.rotation(); 185 | auto Rb = Tb.rotation(); 186 | bool valid = true; 187 | for (int i = 0; i < Ra.rows(); ++i) 188 | { 189 | for (int j = 0; j < Ra.cols(); ++j) 190 | { 191 | if (std::abs(Ra(i, j) - Rb(i, j)) > TOLERANCE) 192 | { 193 | ROS_ERROR_NAMED(LOGNAME, "Pose orientation error on element (%d, %d).", i, j); 194 | ROS_ERROR_NAMED(LOGNAME, "opw: %f, moveit: %f.", Ra(i, j), Rb(i, j)); 195 | valid = false; 196 | } 197 | } 198 | } 199 | 200 | auto pa = Ta.translation(); 201 | auto pb = Tb.translation(); 202 | for (int i = 0; i < 3; ++i) 203 | { 204 | if (std::abs(pa(i) - pb(i)) > TOLERANCE) 205 | { 206 | ROS_ERROR_NAMED(LOGNAME, "Pose position error on element (%d).", i); 207 | ROS_ERROR_NAMED(LOGNAME, "opw: %f, moveit: %f.", pa(i), pb(i)); 208 | valid = false; 209 | } 210 | } 211 | return valid; 212 | } 213 | 214 | bool MoveItOPWKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, 215 | const std::vector& ik_seed_state, std::vector& solution, 216 | moveit_msgs::MoveItErrorCodes& error_code, 217 | const kinematics::KinematicsQueryOptions& options) const 218 | { 219 | const IKCallbackFn solution_callback = 0; 220 | std::vector consistency_limits; 221 | 222 | return searchPositionIK(ik_pose, ik_seed_state, default_timeout_, solution, solution_callback, error_code, 223 | consistency_limits, options); 224 | } 225 | 226 | bool MoveItOPWKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, 227 | const std::vector& ik_seed_state, double timeout, 228 | std::vector& solution, 229 | moveit_msgs::MoveItErrorCodes& error_code, 230 | const kinematics::KinematicsQueryOptions& options) const 231 | { 232 | const IKCallbackFn solution_callback = 0; 233 | std::vector consistency_limits; 234 | 235 | return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits, 236 | options); 237 | } 238 | 239 | bool MoveItOPWKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, 240 | const std::vector& ik_seed_state, double timeout, 241 | const std::vector& consistency_limits, 242 | std::vector& solution, 243 | moveit_msgs::MoveItErrorCodes& error_code, 244 | const kinematics::KinematicsQueryOptions& options) const 245 | { 246 | const IKCallbackFn solution_callback = 0; 247 | return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits, 248 | options); 249 | } 250 | 251 | bool MoveItOPWKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, 252 | const std::vector& ik_seed_state, double timeout, 253 | std::vector& solution, const IKCallbackFn& solution_callback, 254 | moveit_msgs::MoveItErrorCodes& error_code, 255 | const kinematics::KinematicsQueryOptions& options) const 256 | { 257 | std::vector consistency_limits; 258 | return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits, 259 | options); 260 | } 261 | 262 | bool MoveItOPWKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, 263 | const std::vector& ik_seed_state, double timeout, 264 | const std::vector& consistency_limits, 265 | std::vector& solution, const IKCallbackFn& solution_callback, 266 | moveit_msgs::MoveItErrorCodes& error_code, 267 | const kinematics::KinematicsQueryOptions& options) const 268 | { 269 | return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits, 270 | options); 271 | } 272 | 273 | bool MoveItOPWKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, 274 | const std::vector& ik_seed_state, double timeout, 275 | std::vector& solution, const IKCallbackFn& solution_callback, 276 | moveit_msgs::MoveItErrorCodes& error_code, 277 | const std::vector& consistency_limits, 278 | const kinematics::KinematicsQueryOptions& options) const 279 | { 280 | // Convert single pose into a vector of one pose 281 | std::vector ik_poses; 282 | ik_poses.push_back(ik_pose); 283 | 284 | return searchPositionIK(ik_poses, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code, 285 | options); 286 | } 287 | 288 | void MoveItOPWKinematicsPlugin::expandIKSolutions(std::vector>& solutions) const 289 | { 290 | const std::vector& ajms = joint_model_group_->getActiveJointModels(); 291 | for (size_t i = 0; i < ajms.size(); ++i) 292 | { 293 | const robot_model::JointModel* jm = ajms[i]; 294 | if (jm->getVariableBounds().size() > 0) 295 | { 296 | for (auto& bounds : jm->getVariableBounds()) 297 | { 298 | // todo: what to do about continuous joints 299 | if (!bounds.position_bounded_) 300 | continue; 301 | 302 | std::vector> additional_solutions; 303 | for (auto& sol : solutions) 304 | { 305 | std::vector down_sol(sol); 306 | while (down_sol[i] - 2.0 * M_PI > bounds.min_position_) 307 | { 308 | down_sol[i] -= 2.0 * M_PI; 309 | additional_solutions.push_back(down_sol); 310 | } 311 | std::vector up_sol(sol); 312 | while (up_sol[i] + 2.0 * M_PI < bounds.max_position_) 313 | { 314 | up_sol[i] += 2.0 * M_PI; 315 | additional_solutions.push_back(up_sol); 316 | } 317 | } 318 | ROS_DEBUG_STREAM_NAMED(LOGNAME, 319 | "Found " << additional_solutions.size() << " additional solutions for j=" << i + 1); 320 | solutions.insert(solutions.end(), additional_solutions.begin(), additional_solutions.end()); 321 | } 322 | } 323 | } 324 | } 325 | 326 | bool MoveItOPWKinematicsPlugin::searchPositionIK(const std::vector& ik_poses, 327 | const std::vector& ik_seed_state, double /*timeout*/, 328 | const std::vector& /*consistency_limits*/, 329 | std::vector& solution, const IKCallbackFn& solution_callback, 330 | moveit_msgs::MoveItErrorCodes& error_code, 331 | const kinematics::KinematicsQueryOptions& /*options*/) const 332 | { 333 | // Check if active 334 | if (!active_) 335 | { 336 | ROS_ERROR_NAMED(LOGNAME, "kinematics not active"); 337 | error_code.val = error_code.NO_IK_SOLUTION; 338 | return false; 339 | } 340 | 341 | // Check if seed state correct 342 | if (ik_seed_state.size() != dimension_) 343 | { 344 | ROS_ERROR_STREAM_NAMED(LOGNAME, 345 | "Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size()); 346 | error_code.val = error_code.NO_IK_SOLUTION; 347 | return false; 348 | } 349 | 350 | // Check that we have the same number of poses as tips 351 | if (tip_frames_.size() != ik_poses.size()) 352 | { 353 | ROS_ERROR_STREAM_NAMED(LOGNAME, "Mismatched number of pose requests (" << ik_poses.size() << ") to tip frames (" 354 | << tip_frames_.size() 355 | << ") in searchPositionIK"); 356 | error_code.val = error_code.NO_IK_SOLUTION; 357 | return false; 358 | } 359 | 360 | Eigen::Isometry3d pose; 361 | tf::poseMsgToEigen(ik_poses[0], pose); 362 | std::vector> solutions; 363 | if (!getAllIK(pose, solutions)) 364 | { 365 | ROS_DEBUG_STREAM_NAMED(LOGNAME, "Failed to find IK solution"); 366 | error_code.val = error_code.NO_IK_SOLUTION; 367 | return false; 368 | } 369 | 370 | // for all solutions, check if solution +-360° is still inside limits 371 | // An opw solution might be outside the joint limits, while the extended one is inside (e.g. asymmetric limits) 372 | // therefore first extend solution space, then apply joint limits later 373 | expandIKSolutions(solutions); 374 | 375 | ROS_DEBUG_STREAM_NAMED(LOGNAME, "Now have " << solutions.size() << " potential solutions"); 376 | 377 | std::vector limit_obeying_solutions; 378 | // in order to stay threadsafe, instantiate robot state for each thread 379 | static thread_local robot_state::RobotState robot_state(robot_model_); 380 | 381 | for (auto& sol : solutions) 382 | { 383 | robot_state.setJointGroupPositions(joint_model_group_, sol); 384 | // robot_state.update(); // not required for checking bounds 385 | if (!robot_state.satisfiesBounds(joint_model_group_)) 386 | { 387 | ROS_DEBUG_STREAM_NAMED(LOGNAME, "Solution is outside bounds"); 388 | continue; 389 | } 390 | limit_obeying_solutions.push_back({ sol, distance(sol, ik_seed_state) }); 391 | } 392 | 393 | if (limit_obeying_solutions.empty()) 394 | { 395 | ROS_DEBUG_NAMED(LOGNAME, "None of the solutions is within joint limits"); 396 | return false; 397 | } 398 | 399 | ROS_DEBUG_STREAM_NAMED(LOGNAME, "Solutions within limits: " << limit_obeying_solutions.size()); 400 | 401 | // sort solutions by distance to seed state 402 | std::sort(limit_obeying_solutions.begin(), limit_obeying_solutions.end()); 403 | 404 | if (!solution_callback) 405 | { 406 | solution = limit_obeying_solutions.front().value; 407 | return true; 408 | } 409 | 410 | for (auto& sol : limit_obeying_solutions) 411 | { 412 | solution_callback(ik_poses[0], sol.value, error_code); 413 | if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) 414 | { 415 | solution = sol.value; 416 | ROS_DEBUG_STREAM_NAMED(LOGNAME, "Solution passes callback"); 417 | return true; 418 | } 419 | } 420 | 421 | ROS_DEBUG_STREAM_NAMED(LOGNAME, "No solution fullfilled requirements of solution callback"); 422 | return false; 423 | } 424 | 425 | bool MoveItOPWKinematicsPlugin::getPositionIK(const std::vector& ik_poses, 426 | const std::vector& /*ik_seed_state*/, 427 | std::vector>& solutions, KinematicsResult& /*result*/, 428 | const kinematics::KinematicsQueryOptions& /*options*/) const 429 | { 430 | if (ik_poses.size() > 1 || ik_poses.size() == 0) 431 | { 432 | ROS_ERROR_STREAM_NAMED(LOGNAME, "You can only get all solutions for a single pose."); 433 | return false; 434 | } 435 | Eigen::Isometry3d pose; 436 | tf::poseMsgToEigen(ik_poses[0], pose); 437 | return getAllIK(pose, solutions); 438 | } 439 | 440 | bool MoveItOPWKinematicsPlugin::getPositionFK(const std::vector& link_names, 441 | const std::vector& joint_angles, 442 | std::vector& poses) const 443 | { 444 | if (!active_) 445 | { 446 | ROS_ERROR_NAMED(LOGNAME, "kinematics not active"); 447 | return false; 448 | } 449 | poses.resize(link_names.size()); 450 | // Check for size 6, the only robot type that works for the opw_kinematics solver 451 | if (joint_angles.size() != 6) 452 | { 453 | ROS_ERROR_NAMED(LOGNAME, "Joint angles vector must have size: %d", dimension_); 454 | return false; 455 | } 456 | 457 | // Check that we have the same number of poses as tips 458 | if (tip_frames_.size() != poses.size()) 459 | { 460 | ROS_ERROR_STREAM_NAMED(LOGNAME, "Mismatched number of pose requests (" << poses.size() << ") to tip frames (" 461 | << tip_frames_.size() 462 | << ") in searchPositionFK"); 463 | return false; 464 | } 465 | 466 | // opw_kinematics works with std::array 467 | std::array joint_angles_arr; 468 | std::copy(joint_angles.begin(), joint_angles.end(), joint_angles_arr.begin()); 469 | tf::poseEigenToMsg(opw_kinematics::forward(opw_parameters_, joint_angles_arr), poses[0]); 470 | 471 | return true; 472 | } 473 | 474 | const std::vector& MoveItOPWKinematicsPlugin::getJointNames() const 475 | { 476 | return ik_group_info_.joint_names; 477 | } 478 | 479 | const std::vector& MoveItOPWKinematicsPlugin::getLinkNames() const 480 | { 481 | return ik_group_info_.link_names; 482 | } 483 | 484 | const std::vector& MoveItOPWKinematicsPlugin::getVariableNames() const 485 | { 486 | return joint_model_group_->getVariableNames(); 487 | } 488 | 489 | bool MoveItOPWKinematicsPlugin::setOPWParameters() 490 | { 491 | ROS_INFO_STREAM("Getting kinematic parameters from parameter server."); 492 | 493 | ros::NodeHandle nh; 494 | 495 | std::map geometric_parameters; 496 | if (!lookupParam("opw_kinematics_geometric_parameters", geometric_parameters, {})) 497 | { 498 | ROS_ERROR_STREAM("Failed to load geometric parameters for ik solver."); 499 | return false; 500 | } 501 | 502 | std::vector joint_offsets; 503 | if (!lookupParam("opw_kinematics_joint_offsets", joint_offsets, {})) 504 | { 505 | ROS_ERROR_STREAM("Failed to load joint offsets for ik solver."); 506 | return false; 507 | } 508 | 509 | std::vector joint_sign_corrections; 510 | if (!lookupParam("opw_kinematics_joint_sign_corrections", joint_sign_corrections, {})) 511 | { 512 | ROS_ERROR_STREAM("Failed to load joint sign corrections for ik solver."); 513 | return false; 514 | } 515 | 516 | opw_parameters_.a1 = geometric_parameters["a1"]; 517 | opw_parameters_.a2 = geometric_parameters["a2"]; 518 | opw_parameters_.b = geometric_parameters["b"]; 519 | opw_parameters_.c1 = geometric_parameters["c1"]; 520 | opw_parameters_.c2 = geometric_parameters["c2"]; 521 | opw_parameters_.c3 = geometric_parameters["c3"]; 522 | opw_parameters_.c4 = geometric_parameters["c4"]; 523 | 524 | if (joint_offsets.size() != 6) 525 | { 526 | ROS_ERROR_STREAM("Expected joint_offsets to contain 6 elements, but it has " << joint_offsets.size() << "."); 527 | return false; 528 | } 529 | 530 | if (joint_sign_corrections.size() != 6) 531 | { 532 | ROS_ERROR_STREAM("Expected joint_sign_corrections to contain 6 elements, but it has " 533 | << joint_sign_corrections.size() << "."); 534 | return false; 535 | } 536 | 537 | for (std::size_t i = 0; i < joint_offsets.size(); ++i) 538 | { 539 | opw_parameters_.offsets[i] = joint_offsets[i]; 540 | opw_parameters_.sign_corrections[i] = static_cast(joint_sign_corrections[i]); 541 | } 542 | 543 | ROS_INFO_STREAM("Loaded parameters for ik solver:\n" << opw_parameters_); 544 | 545 | return true; 546 | } 547 | 548 | double MoveItOPWKinematicsPlugin::distance(const std::vector& a, const std::vector& b) 549 | { 550 | double cost = 0.0; 551 | for (size_t i = 0; i < a.size(); ++i) 552 | cost += std::abs(b[i] - a[i]); 553 | return cost; 554 | } 555 | 556 | // Compute the index of the closest joint pose in 'candidates' from 'target' 557 | std::size_t MoveItOPWKinematicsPlugin::closestJointPose(const std::vector& target, 558 | const std::vector>& candidates) 559 | { 560 | size_t closest = 0; // index into candidates 561 | double lowest_cost = std::numeric_limits::max(); 562 | for (size_t i = 0; i < candidates.size(); ++i) 563 | { 564 | assert(target.size() == candidates[i].size()); 565 | double c = distance(target, candidates[i]); 566 | if (c < lowest_cost) 567 | { 568 | closest = i; 569 | lowest_cost = c; 570 | } 571 | } 572 | return closest; 573 | } 574 | 575 | bool MoveItOPWKinematicsPlugin::getAllIK(const Eigen::Isometry3d& pose, 576 | std::vector>& joint_poses) const 577 | { 578 | joint_poses.clear(); 579 | 580 | // Transform input pose 581 | // needed if we introduce a tip frame different from tool0 582 | // or a different base frame 583 | // Eigen::Isometry3d tool_pose = diff_base.inverse() * pose * 584 | // tip_frame.inverse(); 585 | 586 | std::array, 8> sols = opw_kinematics::inverse(opw_parameters_, pose); 587 | 588 | // Check the output 589 | std::vector tmp(6); // temporary storage to convert std::array to std::vector 590 | for (auto sol : sols) 591 | { 592 | if (opw_kinematics::isValid(sol)) 593 | { 594 | opw_kinematics::harmonizeTowardZero(sol); 595 | 596 | // TODO: make this better... 597 | std::copy(sol.begin(), sol.end(), tmp.begin()); 598 | // if (isValid(tmp)) 599 | // { 600 | joint_poses.push_back(tmp); 601 | // } 602 | } 603 | } 604 | 605 | return joint_poses.size() > 0; 606 | } 607 | 608 | bool MoveItOPWKinematicsPlugin::getIK(const Eigen::Isometry3d& pose, const std::vector& seed_state, 609 | std::vector& joint_pose) const 610 | { 611 | // Descartes Robot Model interface calls for 'closest' point to seed position 612 | std::vector> joint_poses; 613 | if (!getAllIK(pose, joint_poses)) 614 | return false; 615 | // Find closest joint pose; getAllIK() does isValid checks already 616 | joint_pose = joint_poses[closestJointPose(seed_state, joint_poses)]; 617 | return true; 618 | } 619 | 620 | } // namespace moveit_opw_kinematics_plugin 621 | -------------------------------------------------------------------------------- /test/faulty_kinematics_fanuc.yaml: -------------------------------------------------------------------------------- 1 | robot_description_kinematics: 2 | manipulator: 3 | kinematics_solver: moveit_opw_kinematics_plugin/MoveItOPWKinematicsPlugin 4 | opw_kinematics_geometric_parameters: 5 | a1: 0.150 6 | a2: -0.200 7 | b: 0.000 8 | c1: 0.450 9 | c2: 0.600 10 | c3: 0.640 11 | c4: 0.100 12 | opw_kinematics_joint_offsets: [0, 0, -1.57079632679, 0, 0, 0] 13 | opw_kinematics_joint_sign_corrections: [1, 1, -1, -1, -1, -1] -------------------------------------------------------------------------------- /test/faulty_kinematics_kuka.yaml: -------------------------------------------------------------------------------- 1 | robot_description_kinematics: 2 | manipulator: 3 | kinematics_solver: moveit_opw_kinematics_plugin/MoveItOPWKinematicsPlugin 4 | opw_kinematics_geometric_parameters: 5 | a1: 0.025 6 | a2: -0.035 7 | b: 0.000 8 | c1: 0.400 9 | c2: 0.315 10 | c3: 0.365 11 | c4: 0.081 12 | opw_kinematics_joint_offsets: [0.0, -1.57079632679, 0, 0, 0, 0] 13 | opw_kinematics_joint_sign_corrections: [-1, 1, 1, -1, 1, -1] -------------------------------------------------------------------------------- /test/kinematics_fanuc.yaml: -------------------------------------------------------------------------------- 1 | robot_description_kinematics: 2 | manipulator: 3 | kinematics_solver: moveit_opw_kinematics_plugin/MoveItOPWKinematicsPlugin 4 | opw_kinematics_geometric_parameters: 5 | a1: 0.150 6 | a2: -0.200 7 | b: 0.000 8 | c1: 0.450 9 | c2: 0.600 10 | c3: 0.640 11 | c4: 0.100 12 | opw_kinematics_joint_offsets: [0, 0, -1.57079632679, 0, 0, 3.14159265359] 13 | opw_kinematics_joint_sign_corrections: [1, 1, -1, -1, -1, -1] -------------------------------------------------------------------------------- /test/test_fanuc.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "test_utils.h" 11 | 12 | // Names to read values from the parameter server 13 | const std::string GROUP_PARAM = "group"; 14 | const std::string TIP_LINK_PARAM = "tip_link"; 15 | const std::string ROOT_LINK_PARAM = "root_link"; 16 | 17 | // Robot description almost always called "robot_description" and therefore hardcoded below 18 | const std::string ROBOT_DESCRIPTION = "robot_description"; 19 | 20 | // unused dummy value for kinematics solver instance 21 | const double DEFAULT_SEARCH_DISCRETIZATION = 0.01f; 22 | 23 | /** \Brief Check ik of opw_kinematics against fk of MoveIt! kinematics. 24 | * 25 | * In this test fixure the aim is to calculate inverse kinematics using opw_kinematics 26 | * inside the plugin and check this using robot_state.getGlobalLinkTransform("tip_link"). 27 | */ 28 | class TestKinematicsFanuc : public ::testing::Test 29 | { 30 | protected: 31 | void SetUp() override 32 | { 33 | ros::NodeHandle pnh("~"); 34 | if (pnh.getParam(GROUP_PARAM, group_name_) && pnh.getParam(ROOT_LINK_PARAM, root_link_) && 35 | pnh.getParam(TIP_LINK_PARAM, tip_link_)) 36 | { 37 | robot_model_loader::RobotModelLoader robot_model_loader(ROBOT_DESCRIPTION); 38 | robot_model_ = robot_model_loader.getModel(); 39 | robot_state_.reset(new robot_state::RobotState(robot_model_)); 40 | robot_state_->setToDefaultValues(); 41 | joint_model_group_ = robot_model_->getJointModelGroup(group_name_); 42 | } 43 | else 44 | { 45 | ROS_ERROR_STREAM("Failed to load parameters necessary to load plugin."); 46 | } 47 | } 48 | void TearDown() override 49 | { 50 | } 51 | 52 | std::string root_link_; 53 | std::string tip_link_; 54 | std::string group_name_; 55 | 56 | robot_model::RobotModelPtr robot_model_; 57 | robot_state::RobotStatePtr robot_state_; 58 | robot_model::JointModelGroup* joint_model_group_; 59 | }; 60 | 61 | TEST_F(TestKinematicsFanuc, InitOk) 62 | { 63 | ASSERT_EQ(robot_model_->getJointModelGroupNames()[0], group_name_); 64 | } 65 | 66 | /** \Brief This checks ik against fk for a single joint pose 67 | * */ 68 | TEST_F(TestKinematicsFanuc, TestAllIkSingleJointPose) 69 | { 70 | const std::vector joint_angles = { 0, 0.1, 0.2, 0.3, 0.4, 0.5 }; 71 | robot_state_->setJointGroupPositions(joint_model_group_, joint_angles); 72 | 73 | // find reachable pose 74 | auto fk_pose = robot_state_->getGlobalLinkTransform(tip_link_); 75 | 76 | // type conversions 77 | geometry_msgs::Pose fk_pose_msgs; 78 | tf::poseEigenToMsg(fk_pose, fk_pose_msgs); 79 | const std::vector fk_poses = { fk_pose_msgs }; 80 | 81 | // calculate all ik solutions for the pose in fk_poses 82 | std::vector > solutions; 83 | kinematics::KinematicsResult result; 84 | kinematics::KinematicsQueryOptions options; 85 | const auto solver = joint_model_group_->getSolverInstance(); 86 | 87 | solver->initialize(ROBOT_DESCRIPTION, group_name_, root_link_, tip_link_, DEFAULT_SEARCH_DISCRETIZATION); 88 | 89 | bool success = solver->getPositionIK(fk_poses, joint_angles, solutions, result, options); 90 | ASSERT_TRUE(success); 91 | 92 | std::size_t num_solutions = solutions.size(); 93 | ASSERT_GT(num_solutions, 0); 94 | 95 | // check if fk for all this solutions gives the same pose 96 | Eigen::Isometry3d actual_pose; 97 | for (auto js : solutions) 98 | { 99 | robot_state_->setJointGroupPositions(joint_model_group_, js); 100 | actual_pose = robot_state_->getGlobalLinkTransform(tip_link_); 101 | moveit_opw_kinematics_plugin::testing::comparePoses(actual_pose, fk_pose); 102 | } 103 | } 104 | 105 | int main(int argc, char** argv) 106 | { 107 | ros::init(argc, argv, "moveit_opw_kinematics_test_fanuc"); 108 | testing::InitGoogleTest(&argc, argv); 109 | return RUN_ALL_TESTS(); 110 | } -------------------------------------------------------------------------------- /test/test_fanuc.rostest: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /test/test_fanuc_faulty_params.rostest: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /test/test_faulty_params.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | // Names to read values from the parameter server 11 | const std::string GROUP_PARAM = "group"; 12 | const std::string TIP_LINK_PARAM = "tip_link"; 13 | const std::string ROOT_LINK_PARAM = "root_link"; 14 | 15 | // Robot description almost always called "robot_description" and therefore hardcoded below 16 | const std::string ROBOT_DESCRIPTION = "robot_description"; 17 | 18 | class TestPlugin : public ::testing::Test 19 | { 20 | protected: 21 | void SetUp() override 22 | { 23 | ros::NodeHandle nh; 24 | if (nh.getParam(GROUP_PARAM, group_name_) && nh.getParam(ROOT_LINK_PARAM, root_link_) && 25 | nh.getParam(TIP_LINK_PARAM, tip_link_)) 26 | { 27 | rdf_loader::RDFLoader rdf_loader(ROBOT_DESCRIPTION); 28 | const srdf::ModelSharedPtr& srdf = rdf_loader.getSRDF(); 29 | const urdf::ModelInterfaceSharedPtr& urdf_model = rdf_loader.getURDF(); 30 | 31 | if (!urdf_model || !srdf) 32 | { 33 | ROS_ERROR_NAMED("opw", "URDF and SRDF must be loaded for SRV kinematics " 34 | "solver to work."); // TODO: is this true? 35 | return; 36 | } 37 | 38 | robot_model_.reset(new robot_model::RobotModel(urdf_model, srdf)); 39 | } 40 | else 41 | { 42 | ROS_ERROR_STREAM("Failed to load parameters necessary to load plugin."); 43 | } 44 | } 45 | void TearDown() override 46 | { 47 | } 48 | 49 | robot_model::RobotModelPtr robot_model_; 50 | moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin plugin_; 51 | std::string root_link_; 52 | std::string tip_link_; 53 | std::string group_name_; 54 | std::string robot_description_name_; 55 | }; 56 | 57 | /** \Brief Initialize plugin when the kinematic parameters loaded are incorrect. 58 | * 59 | * Therefore initialize should return false. Notice we do not check explicitly 60 | * why it fails, but we assumed that it fails because the faulty parameters 61 | * where loaded by the rostest launch file. 62 | */ 63 | TEST_F(TestPlugin, InitFaulty) 64 | { 65 | // the last parameter specifies "search_discretization", which is not used by the opw plugin 66 | bool res = plugin_.initialize(*robot_model_.get(), group_name_, root_link_, { tip_link_ }, 0.1); 67 | EXPECT_FALSE(res); 68 | } 69 | 70 | int main(int argc, char** argv) 71 | { 72 | ros::init(argc, argv, "moveit_opw_kinematics_test_fanuc"); 73 | testing::InitGoogleTest(&argc, argv); 74 | return RUN_ALL_TESTS(); 75 | } 76 | -------------------------------------------------------------------------------- /test/test_kuka.rostest: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /test/test_kuka_faulty_params.rostest: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /test/test_kuka_specific.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "test_utils.h" 11 | 12 | class TestKukaSpecific : public testing::Test 13 | { 14 | protected: 15 | void SetUp() override 16 | { 17 | rdf_loader::RDFLoader rdf_loader("robot_description"); 18 | const srdf::ModelSharedPtr& srdf = rdf_loader.getSRDF(); 19 | const urdf::ModelInterfaceSharedPtr& urdf_model = rdf_loader.getURDF(); 20 | 21 | robot_model_.reset(new robot_model::RobotModel(urdf_model, srdf)); 22 | plugin_.initialize(*robot_model_.get(), "manipulator", "base_link", { "tool0" }, 0.1); 23 | } 24 | void TearDown() override 25 | { 26 | } 27 | 28 | protected: 29 | robot_model::RobotModelPtr robot_model_; 30 | moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin plugin_; 31 | }; 32 | 33 | /** \Brief check forward kinematics for robot home position 34 | * 35 | * Calculate by hand position and oriention of tool0 when all joint angles are zero 36 | * px = a1 + c2 + c3 + c4 37 | * py = 0 38 | * pz = c1 + a2 39 | */ 40 | TEST_F(TestKukaSpecific, positionFKAllZero) 41 | { 42 | using Eigen::AngleAxisd; 43 | using Eigen::Translation3d; 44 | using Eigen::Vector3d; 45 | 46 | std::vector link_names; 47 | std::vector joint_angles = { 0, 0, 0, 0, 0, 0 }; 48 | std::vector poses; 49 | 50 | plugin_.getPositionFK(plugin_.getLinkNames(), joint_angles, poses); 51 | 52 | Eigen::Isometry3d pose_actual, pose_desired; 53 | tf::poseMsgToEigen(poses[0], pose_actual); 54 | 55 | pose_desired = Translation3d(0.785, 0, 0.435) * AngleAxisd(M_PI_2, Vector3d::UnitY()); 56 | 57 | moveit_opw_kinematics_plugin::testing::comparePoses(pose_actual, pose_desired); 58 | } 59 | 60 | /** \Brief check forward kinematics for a known position 61 | * 62 | * WARNING: Ugly add hoc test 63 | * This test is meant to catch errors in the specified joint_signed_corrections 64 | * of joint 1, 4 and 6. 65 | * We really need more general tests to check for consistency with the URDF. 66 | */ 67 | TEST_F(TestKukaSpecific, positionFKCheckSigns) 68 | { 69 | using Eigen::AngleAxisd; 70 | using Eigen::Translation3d; 71 | using Eigen::Vector3d; 72 | 73 | const double J1 = M_PI_2, J4 = 0.3, J6 = 0.2; 74 | 75 | std::vector link_names; 76 | std::vector joint_angles = { J1, 0, 0, J4, 0, J6 }; 77 | std::vector poses; 78 | 79 | plugin_.getPositionFK(plugin_.getLinkNames(), joint_angles, poses); 80 | 81 | Eigen::Isometry3d pose_actual, pose_desired; 82 | tf::poseMsgToEigen(poses[0], pose_actual); 83 | 84 | // rotation for the joint offset of joint 2 85 | pose_desired = Translation3d(0, 0, 0) * AngleAxisd(M_PI_2, Vector3d::UnitY()); 86 | 87 | // rotate around GLOBAL z to add rotation caused by joint 1 88 | pose_desired = AngleAxisd(-J1, Vector3d::UnitZ()) * pose_desired; 89 | 90 | // rotate two times around LOCAL z-axis for joints J4 and J6 91 | pose_desired = pose_desired * AngleAxisd(-J4, Vector3d::UnitZ()) * AngleAxisd(-J6, Vector3d::UnitZ()); 92 | 93 | // put the frame at the expected position, non-zero y-value 94 | // instead of x caused by the rotation of joint 1 95 | pose_desired = Translation3d(0, -0.785, 0.435) * pose_desired; 96 | 97 | moveit_opw_kinematics_plugin::testing::comparePoses(pose_actual, pose_desired); 98 | } 99 | -------------------------------------------------------------------------------- /test/test_plugin.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "test_utils.h" 11 | 12 | // Names to read values from the parameter server 13 | const std::string GROUP_PARAM = "group"; 14 | const std::string TIP_LINK_PARAM = "tip_link"; 15 | const std::string ROOT_LINK_PARAM = "root_link"; 16 | 17 | // Robot description almost always called "robot_description" and therefore hardcoded below 18 | const std::string ROBOT_DESCRIPTION = "robot_description"; 19 | 20 | /** \Brief Test ik and fk only using the plugin 21 | * 22 | * As the ik and fk are both calculated using the opw_kinematics packages, 23 | * this tests only check if passing values from the plugin to opw_kinematics works, 24 | * and if the ik and fk calculations inside opw_kinematics are consistent for this robot. 25 | */ 26 | class TestPlugin : public ::testing::Test 27 | { 28 | protected: 29 | void SetUp() override 30 | { 31 | ros::NodeHandle nh; 32 | if (nh.getParam(GROUP_PARAM, group_name_) && nh.getParam(ROOT_LINK_PARAM, root_link_) && 33 | nh.getParam(TIP_LINK_PARAM, tip_link_)) 34 | { 35 | rdf_loader::RDFLoader rdf_loader(ROBOT_DESCRIPTION); 36 | const srdf::ModelSharedPtr& srdf = rdf_loader.getSRDF(); 37 | const urdf::ModelInterfaceSharedPtr& urdf_model = rdf_loader.getURDF(); 38 | 39 | if (!urdf_model || !srdf) 40 | { 41 | ROS_ERROR_NAMED("opw", "URDF and SRDF must be loaded for OPW kinematics " 42 | "tests to work."); 43 | return; 44 | } 45 | 46 | robot_model_.reset(new robot_model::RobotModel(urdf_model, srdf)); 47 | 48 | // the last parameter specifies "search_discretization", which is not used by the opw plugin 49 | plugin_.initialize(*robot_model_.get(), group_name_, root_link_, { tip_link_ }, 0.1); 50 | } 51 | else 52 | { 53 | ROS_ERROR_STREAM("Failed to load parameters necessary to load plugin."); 54 | } 55 | } 56 | void TearDown() override 57 | { 58 | } 59 | 60 | robot_model::RobotModelPtr robot_model_; 61 | moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin plugin_; 62 | std::string root_link_; 63 | std::string tip_link_; 64 | std::string group_name_; 65 | std::string robot_description_name_; 66 | }; 67 | 68 | TEST_F(TestPlugin, InitOk) 69 | { 70 | ASSERT_EQ(plugin_.getGroupName(), group_name_); 71 | } 72 | 73 | TEST_F(TestPlugin, CompareIKAndFK) 74 | { 75 | std::vector link_names; 76 | const std::vector joint_angles = { 0, 0.1, 0.2, 0.3, 0.4, 0.5 }; 77 | std::vector poses_out; 78 | 79 | // find reachable pose 80 | plugin_.getPositionFK(plugin_.getLinkNames(), joint_angles, poses_out); 81 | 82 | // calculate all ik solutions for this pose 83 | const std::vector poses_in = { poses_out[0] }; 84 | std::vector > solutions; 85 | kinematics::KinematicsResult result; 86 | bool res = plugin_.getPositionIK(poses_in, joint_angles, solutions, result); 87 | EXPECT_TRUE(res); 88 | 89 | // check if fk for all this solutions gives the same pose 90 | Eigen::Isometry3d actual, desired; 91 | tf::poseMsgToEigen(poses_out[0], desired); 92 | for (auto js : solutions) 93 | { 94 | plugin_.getPositionFK(plugin_.getLinkNames(), js, poses_out); 95 | tf::poseMsgToEigen(poses_out[0], actual); 96 | moveit_opw_kinematics_plugin::testing::comparePoses(actual, desired); 97 | } 98 | } 99 | 100 | int main(int argc, char** argv) 101 | { 102 | ros::init(argc, argv, "moveit_opw_kinematics_test_fanuc"); 103 | testing::InitGoogleTest(&argc, argv); 104 | return RUN_ALL_TESTS(); 105 | } 106 | -------------------------------------------------------------------------------- /test/test_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef MOVEIT_OPW_KINEMATICS_TEST_UTILS_ 2 | #define MOVEIT_OPW_KINEMATICS_TEST_UTILS_ 3 | 4 | #include 5 | 6 | namespace moveit_opw_kinematics_plugin 7 | { 8 | namespace testing 9 | { 10 | const double TOLERANCE = 1e-6; // absolute tolerance for EXPECT_NEAR checks 11 | 12 | template 13 | using Transform = Eigen::Transform; 14 | 15 | /** \brief Compare every element of two transforms. 16 | */ 17 | template 18 | void comparePoses(const Transform& Ta, const Transform& Tb) 19 | { 20 | using Matrix = Eigen::Matrix; 21 | using Vector = Eigen::Matrix; 22 | 23 | Matrix Ra = Ta.rotation(), Rb = Tb.rotation(); 24 | for (int i = 0; i < Ra.rows(); ++i) 25 | { 26 | for (int j = 0; j < Ra.cols(); ++j) 27 | { 28 | EXPECT_NEAR(Ra(i, j), Rb(i, j), TOLERANCE); 29 | } 30 | } 31 | 32 | Vector pa = Ta.translation(), pb = Tb.translation(); 33 | EXPECT_NEAR(pa[0], pb[0], TOLERANCE); 34 | EXPECT_NEAR(pa[1], pb[1], TOLERANCE); 35 | EXPECT_NEAR(pa[2], pb[2], TOLERANCE); 36 | } 37 | } // namespace testing 38 | } // namespace moveit_opw_kinematics_plugin 39 | 40 | #endif --------------------------------------------------------------------------------