├── trac_ik_python ├── COLCON_IGNORE ├── src │ └── trac_ik_python │ │ ├── __init__.py │ │ └── trac_ik.py ├── setup.py ├── package.xml ├── README.md ├── scripts │ ├── test_pkg.py │ └── test_wrapper.py ├── CMakeLists.txt └── swig │ └── trac_ik_wrap.i ├── trac_ik_kinematics_plugin ├── COLCON_IGNORE ├── trac_ik_kinematics_description.xml ├── package.xml ├── CMakeLists.txt ├── README.md ├── include │ └── trac_ik │ │ └── trac_ik_kinematics_plugin.hpp └── src │ └── trac_ik_kinematics_plugin.cpp ├── .gitignore ├── trac_ik ├── CMakeLists.txt └── package.xml ├── trac_ik_examples ├── README.md ├── package.xml ├── CMakeLists.txt ├── launch │ └── pr2_arm.launch.py └── src │ └── ik_tests.cpp ├── .github ├── ISSUE_TEMPLATE │ ├── feature_request.md │ └── bug_report.md └── workflows │ └── trac_ik_ci.yml ├── README.md ├── trac_ik_lib ├── package.xml ├── include │ └── trac_ik │ │ ├── visibility_control.hpp │ │ ├── nlopt_ik.hpp │ │ ├── kdl_tl.hpp │ │ ├── trac_ik.hpp │ │ ├── dual_quaternion.h │ │ └── math3d.h ├── CMakeLists.txt ├── README.md └── src │ ├── kdl_tl.cpp │ ├── trac_ik.cpp │ └── nlopt_ik.cpp └── LICENSE.txt /trac_ik_python/COLCON_IGNORE: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /trac_ik_kinematics_plugin/COLCON_IGNORE: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /trac_ik_python/src/trac_ik_python/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | trac_ik_python/src/trac_ik_python/trac_ik.pyc -------------------------------------------------------------------------------- /trac_ik/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(trac_ik) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | ament_package() 6 | -------------------------------------------------------------------------------- /trac_ik_python/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['trac_ik_python'], 8 | package_dir={'': 'src'} 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /trac_ik_examples/README.md: -------------------------------------------------------------------------------- 1 | ## trac_ik_examples 2 | 3 | This package provides examples programs to use the standalone TRAC-IK solver and related code. 4 | 5 | Currently, there only exists an `ik_tests` program that compares KDL's Pseudoinverse Jacobian IK solver with TRAC-IK. 6 | The [`pr2_arm.launch.py`](./pr2_arm.launch.py) files runs this test on the default PR2 robot's 7-DOF right arm chain. 7 | -------------------------------------------------------------------------------- /trac_ik_kinematics_plugin/trac_ik_kinematics_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A implementation of kinematics as a plugin based on TRAC-IK. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Suggest an idea for this project 4 | title: '' 5 | labels: enhancement 6 | assignees: aprotyas 7 | 8 | --- 9 | 10 | **Is your feature request related to a problem? Please describe.** 11 | A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] 12 | 13 | **Describe the solution you'd like** 14 | A clear and concise description of what you want to happen. 15 | 16 | **Describe alternatives you've considered** 17 | A clear and concise description of any alternative solutions or features you've considered. 18 | 19 | **Implementation considerations** 20 | Please share - if possible - relevant information on how the feature could be implemented and pros and cons of the different solutions. 21 | 22 | **Additional context** 23 | Add any other context or screenshots about the feature request here. 24 | -------------------------------------------------------------------------------- /trac_ik/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | trac_ik 4 | 0.1.0 5 | 6 | The ROS packages in this repository have been forked from TRACLABS' source 7 | tree, to provide - in ROS 2 - an improved alternative Inverse Kinematics 8 | solver to the popular inverse Jacobian methods in KDL 9 | 10 | Abrar Rahman Protyasha 11 | Patrick Beeson 12 | Barrett Ames 13 | BSD 14 | 15 | https://github.com/aprotyas/trac_ik 16 | 17 | ament_cmake 18 | 19 | trac_ik_examples 20 | trac_ik_kinematics_plugin 21 | trac_ik_lib 22 | trac_ik_python 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /trac_ik_examples/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | trac_ik_examples 4 | 0.1.0 5 | This package contains the source code for testing and comparing trac_ik 6 | Abrar Rahman Protyasha 7 | Patrick Beeson 8 | BSD 9 | 10 | ament_cmake 11 | 12 | libnlopt-cxx-dev 13 | orocos_kdl 14 | rclcpp 15 | trac_ik_lib 16 | 17 | libnlopt0 18 | libnlopt-cxx-dev 19 | orocos_kdl 20 | rclcpp 21 | trac_ik_lib 22 | 23 | ament_lint_auto 24 | ament_lint_common 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /trac_ik_python/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | trac_ik_python 4 | 1.6.6 5 | The trac_ik_python package contains a python wrapper using SWIG 6 | for trac_ik_lib 7 | Abrar Rahman Protyasha 8 | Sam Pfeiffer 9 | BSD 10 | 11 | catkin 12 | 13 | rospy 14 | swig 15 | trac_ik_lib 16 | tf_conversions 17 | libnlopt-dev 18 | libnlopt-cxx-dev 19 | 20 | rospy 21 | swig 22 | trac_ik_lib 23 | tf_conversions 24 | tf 25 | libnlopt-dev 26 | libnlopt0 27 | 28 | 29 | -------------------------------------------------------------------------------- /trac_ik_kinematics_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | trac_ik_kinematics_plugin 4 | 1.6.6 5 | A MoveIt! Kinematics plugin using TRAC-IK 6 | Abrar Rahman Protyasha 7 | Patrick Beeson 8 | BSD 9 | 10 | catkin 11 | 12 | moveit_core 13 | pluginlib 14 | roscpp 15 | tf_conversions 16 | trac_ik_lib 17 | libnlopt-dev 18 | libnlopt-cxx-dev 19 | 20 | moveit_core 21 | pluginlib 22 | roscpp 23 | tf_conversions 24 | trac_ik_lib 25 | libnlopt-dev 26 | libnlopt0 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create an issue highlighting a bug 4 | title: '' 5 | labels: bug 6 | assignees: aprotyas 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | A clear and concise description of what the bug is. 12 | 13 | **Required Info:** 14 | - Package: 15 | - 16 | - Operating System: 17 | - 18 | - Installation type: 19 | - 20 | - Version or commit hash: 21 | - 22 | - DDS implementation (if applicable): 23 | - 24 | - ROS 2 library (if applicable): 25 | - 26 | 27 | **To Reproduce** 28 | Steps to reproduce the behavior: 29 | 1. Go to '...' 30 | 2. Click on '....' 31 | 3. Scroll down to '....' 32 | 4. See error 33 | 34 | **Expected behavior** 35 | A clear and concise description of what you expected to happen. 36 | 37 | **Actual behavior** 38 | A clear and concise description of what has actually been observed. 39 | 40 | **Additional context** 41 | Add any other context about the problem here. 42 | -------------------------------------------------------------------------------- /.github/workflows/trac_ik_ci.yml: -------------------------------------------------------------------------------- 1 | name: trac_ik CI 2 | 3 | on: 4 | pull_request: 5 | paths-ignore: ['**.md'] 6 | push: 7 | branches: 8 | - ros2 9 | paths-ignore: ['**.md'] 10 | schedule: 11 | # Triggers the workflow at 00:00 UTC every day 12 | - cron: '0 0 * * *' 13 | 14 | jobs: 15 | trac_ik-CI: 16 | runs-on: ${{ matrix.os }} 17 | strategy: 18 | fail-fast: false 19 | matrix: 20 | os: [ubuntu-20.04] 21 | ros_distribution: [foxy, galactic, rolling] 22 | steps: 23 | - uses: actions/checkout@v2.3.4 24 | 25 | - name: Setup ROS2 26 | uses: ros-tooling/setup-ros@v0.2 27 | with: 28 | use-ros2-testing: true 29 | required-ros-distributions: ${{ matrix.ros_distribution }} 30 | 31 | - name: Build and test trac_ik 32 | uses: ros-tooling/action-ros-ci@v0.2 33 | id: action_ros_ci 34 | with: 35 | package-name: | 36 | trac_ik 37 | trac_ik_examples 38 | trac_ik_lib 39 | target-ros2-distro: ${{ matrix.ros_distribution }} 40 | 41 | - name: Upload Logs 42 | uses: actions/upload-artifact@v2 43 | with: 44 | name: colcon_logs 45 | path: ${{ steps.action_ros_ci.outputs.ros-workspace-directory-name }}/log 46 | if: always() 47 | -------------------------------------------------------------------------------- /trac_ik_kinematics_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(trac_ik_kinematics_plugin) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")) 5 | set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}") 6 | endif() 7 | 8 | find_package(PkgConfig REQUIRED) 9 | pkg_check_modules(pkg_nlopt REQUIRED nlopt) 10 | 11 | find_package(catkin REQUIRED 12 | COMPONENTS 13 | moveit_core 14 | pluginlib 15 | roscpp 16 | tf_conversions 17 | trac_ik_lib 18 | ) 19 | 20 | include_directories( 21 | include 22 | ${catkin_INCLUDE_DIRS} 23 | ${pkg_nlopt_INCLUDE_DIRS} 24 | ) 25 | 26 | catkin_package( 27 | INCLUDE_DIRS 28 | include 29 | CATKIN_DEPENDS 30 | moveit_core 31 | pluginlib 32 | roscpp 33 | tf_conversions 34 | trac_ik_lib 35 | ) 36 | 37 | set(TRAC_IK_LIBRARY_NAME trac_ik_kinematics_plugin) 38 | 39 | add_library(${TRAC_IK_LIBRARY_NAME} src/trac_ik_kinematics_plugin.cpp) 40 | target_link_libraries(${TRAC_IK_LIBRARY_NAME} ${catkin_LIBRARIES} ${pkg_nlopt_LIBRARIES}) 41 | 42 | install(DIRECTORY include/ 43 | DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} 44 | ) 45 | 46 | install(TARGETS ${TRAC_IK_LIBRARY_NAME} 47 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 48 | 49 | install( 50 | FILES 51 | trac_ik_kinematics_description.xml 52 | DESTINATION 53 | ${CATKIN_PACKAGE_SHARE_DESTINATION} 54 | ) 55 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # trac_ik 2 | 3 | ROS 2 port of `trac_ik`, an alternative Inverse Kinematics solver to the popular inverse Jacobian methods in KDL. 4 | 5 | This repo contains 5 ROS 2 pacakges: 6 | - [`trac_ik`](https://github.com/aprotyas/trac_ik/tree/ros2/trac_ik): A "metapackage" for the examples, C++/Python libraries, and the MoveIt! plugin. 7 | - [`trac_ik_examples`](https://github.com/aprotyas/trac_ik/tree/ros2/trac_ik_examples): Contains an example on how to use the standalone TRAC-IK C++ library. 8 | - [`trac_ik_lib`](https://github.com/aprotyas/trac_ik/tree/ros2/trac_ik_lib): The TRAC-IK kinematics code, builds a .so library that can be used as a drop in replacement for KDL's IK functions for KDL chains. 9 | Details for use are in [`trac_ik_lib/README.md`](https://github.com/aprotyas/trac_ik/tree/ros2/trac_ik_lib/README.md). 10 | - [`trac_ik_kinematics_plugin`](https://github.com/aprotyas/trac_ik/tree/ros2/trac_ik_kinematics_plugin): Provides a MoveIt! plugin that can replace the default KDL plugin for MoveIt! with TRAC-IK for use in planning. 11 | Details for use are in [`trac_ik_kinematics_plugin/README.md`](https://github.com/aprotyas/trac_ik/tree/ros2/trac_ik_kinematics_plugin/README.md). 12 | - [`trac_ik_python`](https://github.com/aprotyas/trac_ik/tree/ros2/trac_ik_python): SWIG based Python wrapper to use TRAC-IK. 13 | Details for use are in [`trac_ik_python/README.md`](https://github.com/aprotyas/trac_ik/tree/ros2/trac_ik_python/README.md). 14 | 15 | Link to the original repository: https://bitbucket.org/traclabs/trac_ik/src/master/ 16 | -------------------------------------------------------------------------------- /trac_ik_lib/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | trac_ik_lib 4 | 0.1.0 5 | 6 | TRAC-IK is a faster, significantly more reliable drop-in replacement for 7 | KDL's pseudoinverse Jacobian solver. 8 | 9 | The TRAC-IK library has a very similar API to KDL's IK solver calls, 10 | except that the user passes a maximum time instead of a maximum number of 11 | search iterations. Additionally, TRAC-IK allows for error tolerances to 12 | be set independently for each Cartesian dimension (x,y,z,roll,pitch.yaw). 13 | 14 | Abrar Rahman Protyasha 15 | Patrick Beeson 16 | Barrett Ames 17 | 18 | BSD 19 | 20 | ament_cmake 21 | eigen3_cmake_module 22 | 23 | eigen 24 | kdl_parser 25 | libnlopt-cxx-dev 26 | rclcpp 27 | urdf 28 | 29 | kdl_parser 30 | libnlopt0 31 | libnlopt-cxx-dev 32 | rclcpp 33 | urdf 34 | 35 | ament_lint_auto 36 | ament_lint_common 37 | 38 | 39 | ament_cmake 40 | 41 | 42 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2008-2021, TRACLabs, Inc. 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /trac_ik_examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12) 2 | project(trac_ik_examples) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++17 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 17) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic -Wshadow -Werror) 16 | endif() 17 | 18 | find_package(ament_cmake REQUIRED) 19 | find_package(NLopt REQUIRED) 20 | find_package(rclcpp REQUIRED) 21 | find_package(orocos_kdl REQUIRED) 22 | find_package(trac_ik_lib REQUIRED) 23 | 24 | add_executable(ik_tests src/ik_tests.cpp) 25 | 26 | # C++20 standard guarantees `std::chrono::system_clock` measures Unix Time 27 | # i.e. `std::chrono::{duration, time_point}` can then replace 28 | # `boost::posix_time::{time_duration, ptime}` 29 | set_target_properties(ik_tests PROPERTIES 30 | CXX_STANDARD 20 31 | CXX_STANDARD_REQUIRED YES 32 | CXX_EXTENSIONS NO 33 | ) 34 | 35 | target_include_directories(ik_tests PUBLIC 36 | ${NLOPT_INCLUDE_DIRS} 37 | ) 38 | 39 | # Needed since NLopt is not an ament package 40 | target_link_libraries(ik_tests 41 | ${NLOPT_LIBRARIES} 42 | ) 43 | 44 | ament_target_dependencies(ik_tests 45 | ament_cmake 46 | rclcpp 47 | orocos_kdl 48 | trac_ik_lib 49 | ) 50 | 51 | install(TARGETS ik_tests 52 | ARCHIVE DESTINATION lib 53 | LIBRARY DESTINATION lib 54 | RUNTIME DESTINATION lib/${PROJECT_NAME} 55 | ) 56 | 57 | install(DIRECTORY launch 58 | DESTINATION share/${PROJECT_NAME} 59 | ) 60 | 61 | if(BUILD_TESTING) 62 | find_package(ament_lint_auto REQUIRED) 63 | ament_lint_auto_find_test_dependencies() 64 | endif() 65 | 66 | ament_package() 67 | -------------------------------------------------------------------------------- /trac_ik_lib/include/trac_ik/visibility_control.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Abrar Rahman Protyasha 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #ifndef TRAC_IK__VISIBILITY_CONTROL_HPP_ 17 | #define TRAC_IK__VISIBILITY_CONTROL_HPP_ 18 | 19 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 20 | // https://gcc.gnu.org/wiki/Visibility 21 | 22 | #if defined _WIN32 || defined __CYGWIN__ 23 | #ifdef __GNUC__ 24 | #define TRAC_IK_EXPORT __attribute__ ((dllexport)) 25 | #define TRAC_IK_IMPORT __attribute__ ((dllimport)) 26 | #else 27 | #define TRAC_IK_EXPORT __declspec(dllexport) 28 | #define TRAC_IK_IMPORT __declspec(dllimport) 29 | #endif 30 | #ifdef TRAC_IK_BUILDING_LIBRARY 31 | #define TRAC_IK_PUBLIC TRAC_IK_EXPORT 32 | #else 33 | #define TRAC_IK_PUBLIC TRAC_IK_IMPORT 34 | #endif 35 | #define TRAC_IK_PUBLIC_TYPE TRAC_IK_PUBLIC 36 | #define TRAC_IK_LOCAL 37 | #else 38 | #define TRAC_IK_EXPORT __attribute__ ((visibility("default"))) 39 | #define TRAC_IK_IMPORT 40 | #if __GNUC__ >= 4 41 | #define TRAC_IK_PUBLIC __attribute__ ((visibility("default"))) 42 | #define TRAC_IK_LOCAL __attribute__ ((visibility("hidden"))) 43 | #else 44 | #define TRAC_IK_PUBLIC 45 | #define TRAC_IK_LOCAL 46 | #endif 47 | #define TRAC_IK_PUBLIC_TYPE 48 | #endif 49 | 50 | #endif // TRAC_IK__VISIBILITY_CONTROL_HPP_ 51 | -------------------------------------------------------------------------------- /trac_ik_lib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12) 2 | project(trac_ik_lib) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++17 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 17) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic -Wshadow -Werror) 16 | endif() 17 | 18 | find_package(ament_cmake REQUIRED) 19 | find_package(eigen3_cmake_module REQUIRED) 20 | find_package(Eigen3 REQUIRED) 21 | find_package(kdl_parser REQUIRED) 22 | find_package(NLopt REQUIRED) 23 | find_package(rclcpp REQUIRED) 24 | find_package(urdf REQUIRED) 25 | 26 | add_library(${PROJECT_NAME} SHARED 27 | src/kdl_tl.cpp 28 | src/nlopt_ik.cpp 29 | src/trac_ik.cpp) 30 | 31 | # C++20 standard guarantees `std::chrono::system_clock` measures Unix Time 32 | # i.e. `std::chrono::{duration, time_point}` can then replace 33 | # `boost::posix_time::{time_duration, ptime}` 34 | set_target_properties(${PROJECT_NAME} PROPERTIES 35 | CXX_STANDARD 20 36 | CXX_STANDARD_REQUIRED YES 37 | CXX_EXTENSIONS NO 38 | ) 39 | 40 | target_include_directories(${PROJECT_NAME} PUBLIC 41 | $ 42 | $ 43 | ${NLOPT_INCLUDE_DIRS} 44 | ) 45 | 46 | target_compile_definitions(${PROJECT_NAME} PRIVATE "TRAC_IK_BUILDING_LIBRARY") 47 | 48 | # Needed since NLopt is not an ament package 49 | target_link_libraries(${PROJECT_NAME} 50 | ${NLOPT_LIBRARIES} 51 | ) 52 | 53 | ament_target_dependencies(${PROJECT_NAME} 54 | ament_cmake 55 | Eigen3 56 | kdl_parser 57 | rclcpp 58 | urdf 59 | ) 60 | 61 | ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) 62 | 63 | ament_export_dependencies( 64 | ament_cmake 65 | kdl_parser 66 | NLopt 67 | rclcpp 68 | urdf 69 | ) 70 | 71 | install(DIRECTORY include/ 72 | DESTINATION include 73 | ) 74 | 75 | install(TARGETS ${PROJECT_NAME} 76 | EXPORT export_${PROJECT_NAME} 77 | ARCHIVE DESTINATION lib 78 | LIBRARY DESTINATION lib 79 | RUNTIME DESTINATION lib/${PROJECT_NAME}) 80 | 81 | if(BUILD_TESTING) 82 | find_package(ament_lint_auto REQUIRED) 83 | list(APPEND AMENT_LINT_AUTO_EXCLUDE 84 | ament_cmake_cppcheck 85 | ament_cmake_uncrustify 86 | ) 87 | 88 | ament_lint_auto_find_test_dependencies() 89 | 90 | # Else there are erroneous results for `.h` headers 91 | ament_cppcheck(LANGUAGE "c++") 92 | ament_uncrustify(LANGUAGE "C++") 93 | endif() 94 | 95 | ament_package() 96 | -------------------------------------------------------------------------------- /trac_ik_examples/launch/pr2_arm.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Abrar Rahman Protyasha 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | 17 | from launch import LaunchDescription 18 | from launch.actions import DeclareLaunchArgument 19 | from launch.substitutions import LaunchConfiguration 20 | from launch_ros.actions import Node 21 | from launch_ros.substitutions import FindPackageShare 22 | 23 | 24 | def generate_launch_description(): 25 | num_samples = LaunchConfiguration('num_samples') 26 | chain_start = LaunchConfiguration('chain_start') 27 | chain_end = LaunchConfiguration('chain_end') 28 | timeout = LaunchConfiguration('timeout') 29 | 30 | pkg_share = FindPackageShare('trac_ik_examples').find('trac_ik_examples') 31 | urdf_file = os.path.join(pkg_share, 'launch', 'pr2.urdf') 32 | with open(urdf_file, 'r') as infp: 33 | robot_desc = infp.read() 34 | 35 | return LaunchDescription( 36 | [ 37 | DeclareLaunchArgument('num_samples', default_value='1000'), 38 | DeclareLaunchArgument('chain_start', default_value='torso_lift_link'), 39 | DeclareLaunchArgument('chain_end', default_value='r_wrist_roll_link'), 40 | DeclareLaunchArgument('timeout', default_value='0.005'), 41 | Node( 42 | package='trac_ik_examples', 43 | executable='ik_tests', 44 | output='screen', 45 | parameters=[ 46 | { 47 | 'robot_description': robot_desc, 48 | 'num_samples': num_samples, 49 | 'chain_start': chain_start, 50 | 'chain_end': chain_end, 51 | 'timeout': timeout, 52 | } 53 | ], 54 | ), 55 | ] 56 | ) 57 | 58 | 59 | """ 60 | Alternative ways to obtain the robot description: 61 | 62 | 1. Using `xacro` with command substitution: 63 | xacro_file = os.path.join(urdf_dir, 'test.urdf.xacro') 64 | robot_desc = launch.substitutions.Command(f'xacro {xacro_file}') 65 | 66 | 2. Using `xacro` API: 67 | xacro_file = os.path.join(urdf_dir, 'test.urdf.xacro') 68 | robot_desc = xacro.process_file(xacro_file).toprettyxml(indent=' ') 69 | 70 | 3. Using `xacro` with `subprocess` utils: 71 | xacro_file = os.path.join(urdf_dir, 'test.urdf.xacro') 72 | p = subprocess.Popen(['xacro', xacro_file], stdout=subprocess.PIPE, stderr=subprocess.PIPE) 73 | robot_desc, stderr = p.communicate() 74 | """ 75 | -------------------------------------------------------------------------------- /trac_ik_kinematics_plugin/README.md: -------------------------------------------------------------------------------- 1 | This package provides is a MoveIt! kinematics plugin that replaces the KDL IK 2 | solver with the TRAC-IK solver. Currently mimic joints are *not* supported. 3 | 4 | ###As of v1.4.3, this package is part of the ROS Indigo/Jade binaries: `sudo apt-get install ros-jade-trac-ik-kinematics-plugin` 5 | 6 | To use: 7 | 8 | - Add this package and trac_ik_lib package to your catkin workspace. 9 | - Find the MoveIt! [kinematics.yaml](http://docs.ros.org/indigo/api/pr2_moveit_tutorials/html/kinematics/src/doc/kinematics_configuration.html) file created for your robot. 10 | - Replace 11 | ```kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin``` 12 | (or similar) with 13 | ```kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin``` 14 | - Set parameters as desired: 15 | - _kinematics\_solver\_timeout_ (timeout in seconds, e.g., 0.005) and _position\_only\_ik_ **ARE** supported. 16 | - _solve\_type_ can be Speed, Distance, Manipulation1, Manipulation2 (see trac\_ik\_lib documentation for details). Default is Speed. 17 | - _kinematics\_solver\_attempts_ parameter is unneeded: unlike KDL, TRAC-IK solver already restarts when it gets stuck 18 | - _kinematics\_solver\_search\_resolution_ is not applicable here. 19 | - Note: The Cartesian error distance used to determine a valid solution is _1e-5_, as that is what is hard-coded into MoveIt's KDL plugin. 20 | 21 | 22 | ###NOTE: My understanding of how MoveIt! works from user experience and looking at the source code (though I am NOT a MoveIt! developer): 23 | 24 | For normal operations, MoveIt! only really calls an IK solver for one pose 25 | (maybe multiple times if the first result is invalid due to self collisions or 26 | the like). This IK solution provides a joint configuration for the 27 | goal. MoveIt! already knows the _current_ joint configuration from encoder 28 | info. All planning at that point is done in _JOINT SPACE_. Collision 29 | detection and constraint checking may use Forward Kinematics to determine the 30 | pose of any subgoal joint configuration, but the planning _IS NOT_ being done 31 | in Cartesian space. After a joint trajectory is found, MoveIt! tries to smooth 32 | the trajectory to make it less "crazy looking", but this does not always 33 | result in a path that is pleasing to human users. 34 | 35 | If you don't have obstacles in your space, you may want to try the Cartesian 36 | planning API in MoveIt! to get "straight-line" motion. MoveIt's Cartesian 37 | planner _IS_ "planning" in Cartesian space (doing lots of IK calls). Though 38 | really it is just performing linear interpolation in x,y,z,roll,pitch,yaw 39 | between the current and desired poses -- it isn't really "searching" for a 40 | solution. That is, MoveIt's Cartesian capability is not doing collision 41 | avoidance or replanning -- if a collision is detected, the linear 42 | interpolation "planning" immediately stops. Unfortunately, in MoveIt! this 43 | scenario still returns _True_ that a trajectory was found. This is not ideal, 44 | and needs to be fixed -- I may write a new Cartesian capability plugin for 45 | MoveIt! that addresses this, but that would be outside the scope of Inverse 46 | Kinematics. 47 | -------------------------------------------------------------------------------- /trac_ik_lib/include/trac_ik/nlopt_ik.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, TRACLabs, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | #ifndef TRAC_IK__NLOPT_IK_HPP_ 31 | #define TRAC_IK__NLOPT_IK_HPP_ 32 | 33 | #include 34 | #include 35 | 36 | #include "nlopt.hpp" 37 | #include "trac_ik/kdl_tl.hpp" 38 | 39 | 40 | namespace NLOPT_IK 41 | { 42 | 43 | enum OptType { Joint, DualQuat, SumSq, L2 }; 44 | 45 | 46 | class NLOPT_IK 47 | { 48 | friend class TRAC_IK::TRAC_IK; 49 | 50 | public: 51 | NLOPT_IK( 52 | const KDL::Chain & chain, const KDL::JntArray & q_min, const KDL::JntArray & q_max, 53 | double maxtime = 0.005, double eps = 1e-3, OptType type = SumSq); 54 | 55 | ~NLOPT_IK() {} 56 | int CartToJnt( 57 | const KDL::JntArray & q_init, const KDL::Frame & p_in, KDL::JntArray & q_out, 58 | const KDL::Twist bounds = KDL::Twist::Zero(), 59 | const KDL::JntArray & q_desired = KDL::JntArray()); 60 | 61 | double minJoints(const std::vector & x, std::vector & grad); 62 | // void cartFourPointError(const std::vector& x, double error[]); 63 | void cartSumSquaredError(const std::vector & x, double error[]); 64 | void cartDQError(const std::vector & x, double error[]); 65 | void cartL2NormError(const std::vector & x, double error[]); 66 | 67 | inline void setMaxtime(double t) 68 | { 69 | maxtime = std::chrono::duration(t); 70 | } 71 | 72 | private: 73 | inline void abort() 74 | { 75 | aborted = true; 76 | } 77 | 78 | inline void reset() 79 | { 80 | aborted = false; 81 | } 82 | 83 | 84 | std::vector lb; 85 | std::vector ub; 86 | 87 | const KDL::Chain chain; 88 | std::vector des; 89 | 90 | 91 | KDL::ChainFkSolverPos_recursive fksolver; 92 | 93 | std::chrono::duration maxtime; 94 | double eps; 95 | int iter_counter; 96 | OptType TYPE; 97 | 98 | KDL::Frame targetPose; 99 | KDL::Frame z_up; 100 | KDL::Frame x_out; 101 | KDL::Frame y_out; 102 | KDL::Frame z_target; 103 | KDL::Frame x_target; 104 | KDL::Frame y_target; 105 | 106 | std::vector types; 107 | 108 | nlopt::opt opt; 109 | 110 | KDL::Frame currentPose; 111 | 112 | std::vector best_x; 113 | int progress; 114 | bool aborted; 115 | 116 | KDL::Twist bounds; 117 | 118 | inline static double fRand(double min, double max) 119 | { 120 | double f = static_cast(rand()) / RAND_MAX; // NOLINT 121 | return min + f * (max - min); 122 | } 123 | }; 124 | 125 | } // namespace NLOPT_IK 126 | 127 | #endif // TRAC_IK__NLOPT_IK_HPP_ 128 | -------------------------------------------------------------------------------- /trac_ik_lib/README.md: -------------------------------------------------------------------------------- 1 | ## trac_ik_lib 2 | 3 | ### API change (WIP) 4 | 5 | Previously, the `TracIK` class could be constructed with a `std::string URDF_param="/robot_description"` argument. 6 | This `URDF_param` argument contained the name of the parameter which housed the robot description URDF string in the global parameter server. 7 | Since ROS 2 does not have "global" parameters, I've decided that the onus of obtaining a robot description XML falls on the `TracIK` callee. 8 | More specifically, this is the new constructor that can ingest a URDF: 9 | 10 | ```c++ 11 | TRAC_IK::TRAC_IK ik_solver( 12 | string base_link, 13 | string tip_link, 14 | string urdf_param=std::string(), 15 | double timeout_in_secs=0.005, 16 | double error=1e-5, 17 | TRAC_IK::SolveType type=TRAC_IK::Speed); 18 | ``` 19 | 20 | And here is a diff of the change: 21 | 22 | ```diff 23 | < string URDF_param="/robot_description", 24 | --- 25 | > string urdf_param=std::string(), 26 | ``` 27 | 28 | **This entails that `urdf_param` is now the entire URDF XML file** (as a string, of course). 29 | Ways to obtain `urdf_param`: 30 | - Subscribing to the `robot_description` topic if there is a `robot_state_publisher` instance in the ROS graph. 31 | - Progammatically read a URDF XML file. 32 | - Have the XML string saved in a parameter for the callee node through a launch script (and `xacro`, if desired...). 33 | 34 | ### Usage 35 | 36 | The TRAC-IK kinematics solver is built in `trac_ik_lib` as a .so library. 37 | The headers and shared objects in this package can be linked against by user programs. 38 | 39 | This requires the Ubuntu packages for NLOpt Libraries to be installed. 40 | This can be done by running `sudo apt-get install libnlopt-dev` on the standard Ubuntu distros. 41 | Alternatively, you can run `rosdep update && rosdep install trac_ik_lib`. 42 | 43 | KDL IK: 44 | 45 | ```c++ 46 | KDL::ChainFkSolverPos_recursive fk_solver(chain); 47 | KDL::ChainIkSolverVel_pinv vik_solver(chain); 48 | KDL::ChainJntToJacSolver jac_solver(chain); 49 | 50 | KDL::ChainIkSolverPos_NR_JL ik_solver( 51 | KDL::Chain chain, 52 | KDL::JntArray lower_joint_limits, 53 | KDL::JntArray upper_joint_limits, 54 | fk_solver, vik_solver, 55 | int num_iterations, 56 | double error); 57 | 58 | int rc = 59 | ik_solver.CartToJnt( 60 | KDL::JntArray joint_seed, 61 | KDL::Frame desired_end_effector_pose, 62 | KDL::JntArray& return_joints); 63 | 64 | // NOTE: CartToJnt succeeded if rc >=0 65 | // 66 | // NOTE: to use a timeout in seconds (e.g., 0.005), the iterations can be set to 1, and this can be called in a loop with your own timer. 67 | // 68 | // NOTE: error == 1e-5 is acceptable for most purposes 69 | ``` 70 | 71 | TRAC-IK: 72 | 73 | ```c++ 74 | #include 75 | 76 | TRAC_IK::TRAC_IK ik_solver( 77 | KDL::Chain chain, 78 | KDL::JntArray lower_joint_limits, 79 | KDL::JntArray upper_joint_limits, 80 | double timeout_in_secs=0.005, 81 | double error=1e-5, 82 | TRAC_IK::SolveType type=TRAC_IK::Speed); 83 | 84 | // OR 85 | 86 | TRAC_IK::TRAC_IK ik_solver( 87 | string base_link, 88 | string tip_link, 89 | string urdf_param=std::string(), 90 | double timeout_in_secs=0.005, 91 | double error=1e-5, 92 | TRAC_IK::SolveType type=TRAC_IK::Speed); 93 | 94 | // NOTE: The last arguments to the constructors are optional. 95 | // The type can be one of the following: 96 | // Speed: returns very quickly the first solution found 97 | // Distance: runs for the full timeout_in_secs, then returns the solution that minimizes SSE from the seed 98 | // Manip1: runs for full timeout, returns solution that maximizes sqrt(det(J*J^T)) (the product of the singular values of the Jacobian) 99 | // Manip2: runs for full timeout, returns solution that minimizes the ratio of min to max singular values of the Jacobian. 100 | 101 | int rc = 102 | ik_solver.CartToJnt( 103 | KDL::JntArray joint_seed, 104 | KDL::Frame desired_end_effector_pose, 105 | KDL::JntArray& return_joints, 106 | KDL::Twist tolerances); 107 | 108 | // NOTE: CartToJnt succeeded if rc >=0 109 | 110 | // NOTE: tolerances on the end effector pose are optional, and if not 111 | // provided, then by default are 0. If given, the ABS() of the 112 | // values will be used to set tolerances at -tol..0..+tol for each of 113 | // the 6 Cartesian dimensions of the end effector pose. 114 | ``` 115 | -------------------------------------------------------------------------------- /trac_ik_lib/include/trac_ik/kdl_tl.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, TRACLabs, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | #ifndef TRAC_IK__KDL_TL_HPP_ 31 | #define TRAC_IK__KDL_TL_HPP_ 32 | 33 | #include 34 | #include 35 | 36 | #include "kdl/chainfksolverpos_recursive.hpp" 37 | #include "kdl/chainiksolvervel_pinv.hpp" 38 | 39 | namespace TRAC_IK 40 | { 41 | class TRAC_IK; 42 | } 43 | 44 | namespace KDL 45 | { 46 | 47 | enum BasicJointType { RotJoint, TransJoint, Continuous }; 48 | 49 | class ChainIkSolverPos_TL 50 | { 51 | friend class TRAC_IK::TRAC_IK; 52 | 53 | public: 54 | ChainIkSolverPos_TL( 55 | const Chain & chain, const JntArray & q_min, const JntArray & q_max, 56 | double maxtime = 0.005, double eps = 1e-3, bool random_restart = false, 57 | bool try_jl_wrap = false); 58 | 59 | ~ChainIkSolverPos_TL(); 60 | 61 | int CartToJnt( 62 | const KDL::JntArray & q_init, const KDL::Frame & p_in, KDL::JntArray & q_out, 63 | const KDL::Twist bounds = KDL::Twist::Zero()); 64 | 65 | inline void setMaxtime(double t) 66 | { 67 | maxtime = std::chrono::duration(t); 68 | } 69 | 70 | private: 71 | const Chain chain; 72 | JntArray q_min; 73 | JntArray q_max; 74 | 75 | KDL::Twist bounds; 76 | 77 | KDL::ChainIkSolverVel_pinv vik_solver; 78 | KDL::ChainFkSolverPos_recursive fksolver; 79 | JntArray delta_q; 80 | std::chrono::duration maxtime; 81 | 82 | double eps; 83 | 84 | bool rr; 85 | bool wrap; 86 | 87 | std::vector types; 88 | 89 | inline void abort() 90 | { 91 | aborted = true; 92 | } 93 | 94 | inline void reset() 95 | { 96 | aborted = false; 97 | } 98 | 99 | bool aborted; 100 | 101 | Frame f; 102 | Twist delta_twist; 103 | 104 | inline static double fRand(double min, double max) 105 | { 106 | double f = static_cast(rand()) / RAND_MAX; // NOLINT 107 | return min + f * (max - min); 108 | } 109 | }; 110 | 111 | /** 112 | * determines the rotation axis necessary to rotate from frame b1 to the 113 | * orientation of frame b2 and the vector necessary to translate the origin 114 | * of b1 to the origin of b2, and stores the result in a Twist 115 | * datastructure. The result is w.r.t. frame b1. 116 | * \param F_a_b1 frame b1 expressed with respect to some frame a. 117 | * \param F_a_b2 frame b2 expressed with respect to some frame a. 118 | * \warning The result is not a real Twist! 119 | * \warning In contrast to standard KDL diff methods, the result of 120 | * diffRelative is w.r.t. frame b1 instead of frame a. 121 | */ 122 | IMETHOD Twist diffRelative(const Frame & F_a_b1, const Frame & F_a_b2, double dt = 1) 123 | { 124 | return Twist( 125 | F_a_b1.M.Inverse() * diff(F_a_b1.p, F_a_b2.p, dt), 126 | F_a_b1.M.Inverse() * diff(F_a_b1.M, F_a_b2.M, dt)); 127 | } 128 | 129 | } // namespace KDL 130 | 131 | #endif // TRAC_IK__KDL_TL_HPP_ 132 | -------------------------------------------------------------------------------- /trac_ik_python/README.md: -------------------------------------------------------------------------------- 1 | # trac_ik_python 2 | 3 | Python wrapper for TRAC IK library using SWIG to generate the bindings. 4 | Please send any questions to [Sam Pfeiffer](mailto:Sammy.Pfeiffer@student.uts.edu.au) 5 | 6 | # Example usage 7 | Upload a robot to the param server: 8 | 9 | roslaunch pr2_description upload_pr2.launch 10 | 11 | Now you can get IK's: 12 | 13 | ```python 14 | #!/usr/bin/env python 15 | 16 | from trac_ik_python.trac_ik import IK 17 | 18 | ik_solver = IK("torso_lift_link", 19 | "r_wrist_roll_link") 20 | 21 | seed_state = [0.0] * ik_solver.number_of_joints 22 | 23 | ik_solver.get_ik(seed_state, 24 | 0.45, 0.1, 0.3, # X, Y, Z 25 | 0.0, 0.0, 0.0, 1.0) # QX, QY, QZ, QW 26 | # Returns: 27 | # (0.537242808640495, 28 | # 0.04673341230604478, 29 | # -0.053508394352190486, 30 | # -1.5099959208163785, 31 | # 2.6007509004432596, 32 | # -1.506431092603137, 33 | # -3.040949079090651) 34 | ``` 35 | 36 | 37 | You can also play with the bounds of the IK call: 38 | ```python 39 | #!/usr/bin/env python 40 | 41 | from trac_ik_python.trac_ik import IK 42 | 43 | ik_solver = IK("torso_lift_link", 44 | "r_wrist_roll_link") 45 | 46 | seed_state = [0.0] * ik_solver.number_of_joints 47 | 48 | ik_solver.get_ik(seed_state, 49 | 0.45, 0.1, 0.3, 50 | 0.0, 0.0, 0.0, 1.0, 51 | 0.01, 0.01, 0.01, # X, Y, Z bounds 52 | 0.1, 0.1, 0.1) # Rotation X, Y, Z bounds 53 | ) 54 | # Returns: 55 | # (0.5646018385887146, 56 | # 0.04759637706046231, 57 | # 0.026629718805901908, 58 | # -1.5106828886580062, 59 | # 2.5541685245726535, 60 | # -1.4663448384900402, 61 | # -3.104163452483634) 62 | ``` 63 | 64 | The coordinate frame of the given poses must be in the base frame, you can check the links/joints being used: 65 | ```python 66 | #!/usr/bin/env python 67 | 68 | from trac_ik_python.trac_ik import IK 69 | 70 | ik_solver = IK("torso_lift_link", 71 | "r_wrist_roll_link") 72 | 73 | ik_solver.base_link 74 | # 'torso_lift_link' 75 | 76 | ik_solver.tip_link 77 | # 'r_wrist_roll_link' 78 | 79 | ik_solver.joint_names 80 | # ('r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint') 81 | 82 | ik_solver.link_names 83 | # ('r_shoulder_pan_link', 'r_shoulder_lift_link', 'r_upper_arm_roll_link', 'r_upper_arm_link', 'r_elbow_flex_link', 'r_forearm_roll_link', 'r_forearm_link', 'r_wrist_flex_link', 'r_wrist_roll_link') 84 | ``` 85 | 86 | You can also initialize the IK from a string containing the URDF (by default it takes it from the `/robot_description` parameter in the param server): 87 | ```python 88 | #!/usr/bin/env python 89 | 90 | from trac_ik_python.trac_ik import IK 91 | 92 | # Get your URDF from somewhere 93 | urdf_str = rospy.get_param('/robot_description') 94 | 95 | ik_solver = IK("torso_lift_link", 96 | "r_wrist_roll_link", 97 | urdf_string=urdf_str) 98 | ``` 99 | 100 | 101 | You can also check and modify the joint limits: 102 | ```python 103 | #!/usr/bin/env python 104 | 105 | from trac_ik_python.trac_ik import IK 106 | 107 | ik_solver = IK("torso_lift_link", 108 | "r_wrist_roll_link") 109 | 110 | lower_bound, upper_bound = ik_solver.get_joint_limits() 111 | # lower_bound: (-2.1353981494903564, -0.35359999537467957, -3.75, -2.121299982070923, -3.4028234663852886e+38, -2.0, -3.4028234663852886e+38) 112 | # upper_bound: (0.5646018385887146, 1.2963000535964966, 0.6499999761581421, -0.15000000596046448, 3.4028234663852886e+38, -0.10000000149011612, 3.4028234663852886e+38) 113 | ik_solver.set_joint_limits([0.0]* ik_solver.number_of_joints, upper_bound) 114 | ``` 115 | 116 | # Extra notes 117 | Given that the Python wrapper is made using [SWIG](http://www.swig.org/) it could be extended to other languages. 118 | 119 | You'll get extra output when instantiating the class that comes from C++: 120 | ``` 121 | [ WARN] [1486091331.089974163]: The root link base_footprint has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF. 122 | ``` 123 | 124 | When finishing the program you'll also get this error (which [is OK apparently](https://github.com/ros-planning/moveit/issues/331)): 125 | ``` 126 | terminate called after throwing an instance of 'boost::exception_detail::clone_impl >' 127 | what(): boost: mutex lock failed in pthread_mutex_lock: Invalid argument 128 | Aborted (core dumped) 129 | ``` 130 | 131 | And as a final note I didn't simplify even more letting Pose/PoseStamped messages as input, or transforming using TF coordinate frames, adding forward kinematics, etc. to keep it simple. 132 | -------------------------------------------------------------------------------- /trac_ik_python/scripts/test_pkg.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from trac_ik_python.trac_ik import IK 4 | from numpy.random import random 5 | import time 6 | 7 | 8 | if __name__ == '__main__': 9 | # roslaunch pr2_description upload_pr2.launch 10 | # Needed beforehand 11 | 12 | ik_solver = IK("torso_lift_link", 13 | "r_wrist_roll_link") 14 | 15 | print("IK solver uses link chain:") 16 | print(ik_solver.link_names) 17 | 18 | print("IK solver base frame:") 19 | print(ik_solver.base_link) 20 | 21 | print("IK solver tip link:") 22 | print(ik_solver.tip_link) 23 | 24 | print("IK solver for joints:") 25 | print(ik_solver.joint_names) 26 | 27 | print("IK solver using joint limits:") 28 | lb, up = ik_solver.get_joint_limits() 29 | print("Lower bound: " + str(lb)) 30 | print("Upper bound: " + str(up)) 31 | 32 | qinit = [0.] * ik_solver.number_of_joints 33 | x = y = z = 0.0 34 | rx = ry = rz = 0.0 35 | rw = 1.0 36 | bx = by = bz = 0.001 37 | brx = bry = brz = 0.1 38 | 39 | # Generate a set of random coords in the arm workarea approx 40 | NUM_COORDS = 200 41 | rand_coords = [] 42 | for _ in range(NUM_COORDS): 43 | x = random() * 0.5 44 | y = random() * 0.6 + -0.3 45 | z = random() * 0.7 + -0.35 46 | rand_coords.append((x, y, z)) 47 | 48 | # Check some random coords with fixed orientation 49 | avg_time = 0.0 50 | num_solutions_found = 0 51 | for x, y, z in rand_coords: 52 | ini_t = time.time() 53 | sol = ik_solver.get_ik(qinit, 54 | x, y, z, 55 | rx, ry, rz, rw, 56 | bx, by, bz, 57 | brx, bry, brz) 58 | fin_t = time.time() 59 | call_time = fin_t - ini_t 60 | # print("IK call took: " + str(call_time)) 61 | avg_time += call_time 62 | if sol: 63 | # print("X, Y, Z: " + str( (x, y, z) )) 64 | # print("SOL: " + str(sol)) 65 | num_solutions_found += 1 66 | avg_time = avg_time / NUM_COORDS 67 | 68 | print() 69 | print("Found " + str(num_solutions_found) + " of 200 random coords") 70 | print("Average IK call time: " + str(avg_time)) 71 | print() 72 | 73 | # Check if orientation bounds work 74 | avg_time = 0.0 75 | num_solutions_found = 0 76 | brx = bry = brz = 9999.0 # We don't care about orientation 77 | for x, y, z in rand_coords: 78 | ini_t = time.time() 79 | sol = ik_solver.get_ik(qinit, 80 | x, y, z, 81 | rx, ry, rz, rw, 82 | bx, by, bz, 83 | brx, bry, brz) 84 | fin_t = time.time() 85 | call_time = fin_t - ini_t 86 | # print("IK call took: " + str(call_time)) 87 | avg_time += call_time 88 | if sol: 89 | # print("X, Y, Z: " + str( (x, y, z) )) 90 | # print("SOL: " + str(sol)) 91 | num_solutions_found += 1 92 | 93 | avg_time = avg_time / NUM_COORDS 94 | print() 95 | print("Found " + str(num_solutions_found) + " of 200 random coords ignoring orientation") 96 | print("Average IK call time: " + str(avg_time)) 97 | print() 98 | 99 | # Check if big position and orientation bounds work 100 | avg_time = 0.0 101 | num_solutions_found = 0 102 | bx = by = bz = 9999.0 103 | brx = bry = brz = 9999.0 104 | for x, y, z in rand_coords: 105 | ini_t = time.time() 106 | sol = ik_solver.get_ik(qinit, 107 | x, y, z, 108 | rx, ry, rz, rw, 109 | bx, by, bz, 110 | brx, bry, brz) 111 | fin_t = time.time() 112 | call_time = fin_t - ini_t 113 | # print("IK call took: " + str(call_time)) 114 | avg_time += call_time 115 | if sol: 116 | # print("X, Y, Z: " + str( (x, y, z) )) 117 | # print("SOL: " + str(sol)) 118 | num_solutions_found += 1 119 | 120 | avg_time = avg_time / NUM_COORDS 121 | 122 | print() 123 | print("Found " + str(num_solutions_found) + " of 200 random coords ignoring everything") 124 | print("Average IK call time: " + str(avg_time)) 125 | print() 126 | 127 | # std::vector CartToJnt(const std::vector q_init, 128 | # const double x, const double y, const double z, 129 | # const double rx, const double ry, const double rz, const double rw, 130 | # // bounds x y z 131 | # const double boundx=0.0, const double boundy=0.0, const double boundz=0.0, 132 | # // bounds on rotation x y z 133 | # const double boundrx=0.0, const double boundry=0.0, const double 134 | # boundrz=0.0){ 135 | -------------------------------------------------------------------------------- /trac_ik_python/src/trac_ik_python/trac_ik.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Author: Sammy Pfeiffer 4 | # Convenience code to wrap TRAC IK 5 | 6 | from trac_ik_python.trac_ik_wrap import TRAC_IK 7 | import rospy 8 | 9 | 10 | class IK(object): 11 | def __init__(self, base_link, tip_link, 12 | timeout=0.005, epsilon=1e-5, solve_type="Speed", 13 | urdf_string=None): 14 | """ 15 | Create a TRAC_IK instance and keep track of it. 16 | 17 | :param str base_link: Starting link of the chain. 18 | :param str tip_link: Last link of the chain. 19 | :param float timeout: Timeout in seconds for the IK calls. 20 | :param float epsilon: Error epsilon. 21 | :param solve_type str: Type of solver, can be: 22 | Speed (default), Distance, Manipulation1, Manipulation2 23 | :param urdf_string str: Optional arg, if not given URDF is taken from 24 | the param server at /robot_description. 25 | """ 26 | if urdf_string is None: 27 | urdf_string = rospy.get_param('/robot_description') 28 | self._urdf_string = urdf_string 29 | self._timeout = timeout 30 | self._epsilon = epsilon 31 | self._solve_type = solve_type 32 | self.base_link = base_link 33 | self.tip_link = tip_link 34 | self._ik_solver = TRAC_IK(self.base_link, 35 | self.tip_link, 36 | self._urdf_string, 37 | self._timeout, 38 | self._epsilon, 39 | self._solve_type) 40 | self.number_of_joints = self._ik_solver.getNrOfJointsInChain() 41 | self.joint_names = self._ik_solver.getJointNamesInChain( 42 | self._urdf_string) 43 | self.link_names = self._ik_solver.getLinkNamesInChain() 44 | 45 | def get_ik(self, qinit, 46 | x, y, z, 47 | rx, ry, rz, rw, 48 | bx=1e-5, by=1e-5, bz=1e-5, 49 | brx=1e-3, bry=1e-3, brz=1e-3): 50 | """ 51 | Do the IK call. 52 | 53 | :param list of float qinit: Initial status of the joints as seed. 54 | :param float x: X coordinates in base_frame. 55 | :param float y: Y coordinates in base_frame. 56 | :param float z: Z coordinates in base_frame. 57 | :param float rx: X quaternion coordinate. 58 | :param float ry: Y quaternion coordinate. 59 | :param float rz: Z quaternion coordinate. 60 | :param float rw: W quaternion coordinate. 61 | :param float bx: X allowed bound. 62 | :param float by: Y allowed bound. 63 | :param float bz: Z allowed bound. 64 | :param float brx: rotation over X allowed bound. 65 | :param float bry: rotation over Y allowed bound. 66 | :param float brz: rotation over Z allowed bound. 67 | 68 | :return: joint values or None if no solution found. 69 | :rtype: tuple of float. 70 | """ 71 | if len(qinit) != self.number_of_joints: 72 | raise Exception("qinit has length %i and it should have length %i" % ( 73 | len(qinit), self.number_of_joints)) 74 | solution = self._ik_solver.CartToJnt(qinit, 75 | x, y, z, 76 | rx, ry, rz, rw, 77 | bx, by, bz, 78 | brx, bry, brz) 79 | if solution: 80 | return solution 81 | else: 82 | return None 83 | 84 | def get_joint_limits(self): 85 | """ 86 | Return lower bound limits and upper bound limits for all the joints 87 | in the order of the joint names. 88 | """ 89 | lb = self._ik_solver.getLowerBoundLimits() 90 | ub = self._ik_solver.getUpperBoundLimits() 91 | return lb, ub 92 | 93 | def set_joint_limits(self, lower_bounds, upper_bounds): 94 | """ 95 | Set joint limits for all the joints. 96 | 97 | :arg list lower_bounds: List of float of the lower bound limits for 98 | all joints. 99 | :arg list upper_bounds: List of float of the upper bound limits for 100 | all joints. 101 | """ 102 | if len(lower_bounds) != self.number_of_joints: 103 | raise Exception("lower_bounds array size mismatch, it's size %i, should be %i" % ( 104 | len(lower_bounds), 105 | self.number_of_joints)) 106 | 107 | if len(upper_bounds) != self.number_of_joints: 108 | raise Exception("upper_bounds array size mismatch, it's size %i, should be %i" % ( 109 | len(upper_bounds), 110 | self.number_of_joints)) 111 | self._ik_solver.setKDLLimits(lower_bounds, upper_bounds) 112 | -------------------------------------------------------------------------------- /trac_ik_python/scripts/test_wrapper.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from trac_ik_python.trac_ik_wrap import TRAC_IK 4 | import rospy 5 | from numpy.random import random 6 | import time 7 | 8 | 9 | if __name__ == '__main__': 10 | # roslaunch pr2_description upload_pr2.launch 11 | # Needed beforehand 12 | urdf = rospy.get_param('/robot_description') 13 | # params of constructor: 14 | # base_link, tip_link, urdf_string, timeout, epsilon, solve_type="Speed" 15 | # solve_type can be: Distance, Speed, Manipulation1, Manipulation2 16 | ik_solver = TRAC_IK("torso_lift_link", 17 | "r_wrist_roll_link", 18 | urdf, 19 | 0.005, # default seconds 20 | 1e-5, # default epsilon 21 | "Speed") 22 | print("Number of joints:") 23 | print(ik_solver.getNrOfJointsInChain()) 24 | print("Joint names:") 25 | print(ik_solver.getJointNamesInChain(urdf)) 26 | print("Link names:") 27 | print(ik_solver.getLinkNamesInChain()) 28 | 29 | qinit = [0.] * 7 30 | x = y = z = 0.0 31 | rx = ry = rz = 0.0 32 | rw = 1.0 33 | bx = by = bz = 0.001 34 | brx = bry = brz = 0.1 35 | 36 | # Generate a set of random coords in the arm workarea approx 37 | NUM_COORDS = 200 38 | rand_coords = [] 39 | for _ in range(NUM_COORDS): 40 | x = random() * 0.5 41 | y = random() * 0.6 + -0.3 42 | z = random() * 0.7 + -0.35 43 | rand_coords.append((x, y, z)) 44 | 45 | # Check some random coords with fixed orientation 46 | avg_time = 0.0 47 | num_solutions_found = 0 48 | for x, y, z in rand_coords: 49 | ini_t = time.time() 50 | sol = ik_solver.CartToJnt(qinit, 51 | x, y, z, 52 | rx, ry, rz, rw, 53 | bx, by, bz, 54 | brx, bry, brz) 55 | fin_t = time.time() 56 | call_time = fin_t - ini_t 57 | # print "IK call took: " + str(call_time) 58 | avg_time += call_time 59 | if sol: 60 | # print "X, Y, Z: " + str( (x, y, z) ) 61 | # print "SOL: " + str(sol) 62 | num_solutions_found += 1 63 | avg_time = avg_time / NUM_COORDS 64 | 65 | print() 66 | print("Found " + str(num_solutions_found) + " of 200 random coords") 67 | print("Average IK call time: " + str(avg_time)) 68 | print() 69 | 70 | # Check if orientation bounds work 71 | avg_time = 0.0 72 | num_solutions_found = 0 73 | brx = bry = brz = 9999.0 # We don't care about orientation 74 | for x, y, z in rand_coords: 75 | ini_t = time.time() 76 | sol = ik_solver.CartToJnt(qinit, 77 | x, y, z, 78 | rx, ry, rz, rw, 79 | bx, by, bz, 80 | brx, bry, brz) 81 | fin_t = time.time() 82 | call_time = fin_t - ini_t 83 | # print("IK call took: " + str(call_time)) 84 | avg_time += call_time 85 | if sol: 86 | # print("X, Y, Z: " + str( (x, y, z) )) 87 | # print("SOL: " + str(sol)) 88 | num_solutions_found += 1 89 | 90 | avg_time = avg_time / NUM_COORDS 91 | print() 92 | print("Found " + str(num_solutions_found) + " of 200 random coords ignoring orientation") 93 | print("Average IK call time: " + str(avg_time)) 94 | print() 95 | 96 | # Check if big position and orientation bounds work 97 | avg_time = 0.0 98 | num_solutions_found = 0 99 | bx = by = bz = 9999.0 100 | brx = bry = brz = 9999.0 101 | for x, y, z in rand_coords: 102 | ini_t = time.time() 103 | sol = ik_solver.CartToJnt(qinit, 104 | x, y, z, 105 | rx, ry, rz, rw, 106 | bx, by, bz, 107 | brx, bry, brz) 108 | fin_t = time.time() 109 | call_time = fin_t - ini_t 110 | # print("IK call took: " + str(call_time)) 111 | avg_time += call_time 112 | if sol: 113 | # print("X, Y, Z: " + str( (x, y, z) )) 114 | # print("SOL: " + str(sol)) 115 | num_solutions_found += 1 116 | 117 | avg_time = avg_time / NUM_COORDS 118 | 119 | print() 120 | print("Found " + str(num_solutions_found) + " of 200 random coords ignoring everything") 121 | print("Average IK call time: " + str(avg_time)) 122 | print() 123 | 124 | # std::vector CartToJnt(const std::vector q_init, 125 | # const double x, const double y, const double z, 126 | # const double rx, const double ry, const double rz, const double rw, 127 | # // bounds x y z 128 | # const double boundx=0.0, const double boundy=0.0, const double boundz=0.0, 129 | # // bounds on rotation x y z 130 | # const double boundrx=0.0, const double boundry=0.0, const double 131 | # boundrz=0.0){ 132 | -------------------------------------------------------------------------------- /trac_ik_python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(trac_ik_python) 3 | 4 | add_compile_options(-std=c++11 -Wno-deprecated) 5 | 6 | ## Find catkin macros and libraries 7 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 8 | ## is used, also find other catkin packages 9 | find_package(catkin REQUIRED COMPONENTS 10 | rospy 11 | trac_ik_lib 12 | tf_conversions 13 | ) 14 | 15 | find_package(PkgConfig REQUIRED) 16 | pkg_check_modules(pkg_nlopt REQUIRED nlopt) 17 | 18 | find_package(SWIG REQUIRED) 19 | 20 | if(DEFINED ENV{ROS_PYTHON_VERSION} AND ENV{ROS_PYTHON_VERSION} EQUAL 3) 21 | # Python 3 22 | find_package(Python3 COMPONENTS Development) 23 | else() 24 | # Python 2 25 | find_package(PythonLibs REQUIRED) 26 | endif() 27 | 28 | catkin_python_setup() 29 | 30 | ################################### 31 | ## catkin specific configuration ## 32 | ################################### 33 | catkin_package() 34 | 35 | ########### 36 | ## Build ## 37 | ########### 38 | 39 | #set(CMAKE_VERBOSE_MAKEFILE ON) 40 | 41 | # SWIG stuff 42 | include(${SWIG_USE_FILE}) 43 | file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/swig) 44 | 45 | # To add trac_ik_lib headers (from this ros answers: http://answers.ros.org/question/201977/include-header-file-from-another-package-indigo/) 46 | include_directories(${catkin_INCLUDE_DIRS} ${pkg_nlopt_INCLUDE_DIRS}) 47 | 48 | 49 | # Python trac_ik wrapper using SWIG 50 | # Based on: https://github.com/kdhansen/Aseta/blob/36a7a104f3430ff8b54416a07062234673a73762/src/gatsp/CMakeLists.txt 51 | SET_SOURCE_FILES_PROPERTIES( 52 | swig/trak_ik_wrap.i PROPERTIES CPLUSPLUS ON 53 | ) 54 | 55 | if(DEFINED ENV{ROS_PYTHON_VERSION} AND ENV{ROS_PYTHON_VERSION} EQUAL 3) 56 | include_directories( 57 | ${Python3_INCLUDE_DIRS} 58 | ${Boost_INCLUDE_DIRS} 59 | ${pkg_nlopt_INCLUDE_DIRS} 60 | ) 61 | else() 62 | include_directories( 63 | ${PYTHON_INCLUDE_DIRS} # This solved Python.h: No such file or directory 64 | ${Boost_INCLUDE_DIRS} 65 | ) 66 | endif() 67 | 68 | 69 | 70 | # To overcome (took me hours to notice): 71 | # /opt/ros/indigo/include/kdl/utilities/utility.h:27:19: fatal error: cstdlib: No such file or directory 72 | # ...build/trac_ik_lib/swig/trac_ik_wrapPYTHON_wrap.c:2977:21: fatal error: stdexcept: No such file or directory 73 | set(CMAKE_C_COMPILER ${CMAKE_CXX_COMPILER}) 74 | 75 | # To force SWIG on generating a C++ file-wrapper so we overcome 76 | # error: expected primary-expression before ‘=’ token 77 | # TRAC_IK_ns = *((namespace *)(argp)); 78 | set(CMAKE_SWIG_FLAGS "-c++") 79 | 80 | # This actually makes the compilation of the generated trac_ik_wrapPYTHON_wrap.c happen 81 | if(${CMAKE_VERSION} VERSION_LESS "3.8.0") 82 | # SWIG_ADD_MODULE is deprecated from 3.8.0 83 | SWIG_ADD_MODULE(trac_ik_wrap python 84 | swig/trac_ik_wrap.i 85 | ) 86 | else() 87 | SWIG_ADD_LIBRARY(trac_ik_wrap 88 | LANGUAGE python 89 | SOURCES swig/trac_ik_wrap.i 90 | ) 91 | endif() 92 | 93 | # Link the wrapper with the actual library 94 | 95 | if(DEFINED ENV{ROS_PYTHON_VERSION} AND ENV{ROS_PYTHON_VERSION} EQUAL 3) 96 | SWIG_LINK_LIBRARIES(trac_ik_wrap ${Python3_LIBRARIES} ${catkin_LIBRARIES}) 97 | else() 98 | SWIG_LINK_LIBRARIES(trac_ik_wrap ${PYTHON_LIBRARIES} ${catkin_LIBRARIES}) 99 | endif() 100 | 101 | # The name '_trac_ik_wrap' is given by SWIG, haven't found how to change it 102 | set_target_properties(_trac_ik_wrap 103 | PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} 104 | ) 105 | 106 | 107 | ############# 108 | ## Install ## 109 | ############# 110 | 111 | add_custom_command(TARGET _trac_ik_wrap POST_BUILD 112 | COMMAND ${CMAKE_COMMAND} -E copy ${CMAKE_CURRENT_BINARY_DIR}/trac_ik_wrap.py ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} 113 | ) 114 | 115 | add_custom_command(TARGET _trac_ik_wrap POST_BUILD 116 | COMMAND touch ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/__init__.py 117 | ) 118 | 119 | # Install the trac_ik_wrap.py generated file 120 | # Needs to be done after creating the python wrapper (usually this is done at the top) 121 | 122 | 123 | install(FILES ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/trac_ik_wrap.py ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/_trac_ik_wrap.so 124 | src/trac_ik_python/trac_ik.py 125 | src/trac_ik_python/__init__.py 126 | DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} 127 | ) 128 | 129 | catkin_install_python(PROGRAMS scripts/test_pkg.py scripts/test_wrapper.py 130 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 131 | 132 | 133 | ############# 134 | ## Testing ## 135 | ############# 136 | 137 | # TODO 138 | 139 | ## Add gtest based cpp test target and link libraries 140 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_trac_ik_python.cpp) 141 | # if(TARGET ${PROJECT_NAME}-test) 142 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 143 | # endif() 144 | 145 | ## Add folders to be run by python nosetests 146 | # catkin_add_nosetests(test) 147 | -------------------------------------------------------------------------------- /trac_ik_lib/include/trac_ik/trac_ik.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, TRACLabs, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | #ifndef TRAC_IK__TRAC_IK_HPP_ 31 | #define TRAC_IK__TRAC_IK_HPP_ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include "kdl/chainjnttojacsolver.hpp" 42 | #include "trac_ik/nlopt_ik.hpp" 43 | #include "trac_ik/visibility_control.hpp" 44 | 45 | namespace TRAC_IK 46 | { 47 | 48 | enum SolveType { Speed, Distance, Manip1, Manip2 }; 49 | 50 | class TRAC_IK_PUBLIC TRAC_IK 51 | { 52 | public: 53 | TRAC_IK_PUBLIC 54 | TRAC_IK( 55 | const KDL::Chain & _chain, const KDL::JntArray & _q_min, const KDL::JntArray & _q_max, 56 | double _maxtime = 0.005, double _eps = 1e-5, SolveType _type = Speed); 57 | 58 | TRAC_IK_PUBLIC 59 | TRAC_IK( 60 | const std::string & base_link, const std::string & tip_link, 61 | const std::string & urdf_xml = "", double _maxtime = 0.005, double _eps = 1e-5, 62 | SolveType _type = Speed); 63 | 64 | TRAC_IK_PUBLIC 65 | ~TRAC_IK(); 66 | 67 | TRAC_IK_PUBLIC 68 | bool getKDLChain(KDL::Chain & chain_) 69 | { 70 | chain_ = chain; 71 | return initialized; 72 | } 73 | 74 | TRAC_IK_PUBLIC 75 | bool getKDLLimits(KDL::JntArray & lb_, KDL::JntArray & ub_) 76 | { 77 | lb_ = lb; 78 | ub_ = ub; 79 | return initialized; 80 | } 81 | 82 | TRAC_IK_PUBLIC 83 | // Requires a previous call to CartToJnt() 84 | bool getSolutions(std::vector & solutions_) 85 | { 86 | solutions_ = solutions; 87 | return initialized && !solutions.empty(); 88 | } 89 | 90 | TRAC_IK_PUBLIC 91 | bool getSolutions( 92 | std::vector & solutions_, std::vector> & errors_) 94 | { 95 | errors_ = errors; 96 | return getSolutions(solutions_); 97 | } 98 | 99 | TRAC_IK_PUBLIC 100 | bool setKDLLimits(KDL::JntArray & lb_, KDL::JntArray & ub_) 101 | { 102 | lb = lb_; 103 | ub = ub_; 104 | nl_solver.reset(new NLOPT_IK::NLOPT_IK(chain, lb, ub, maxtime.count(), eps, NLOPT_IK::SumSq)); 105 | iksolver.reset(new KDL::ChainIkSolverPos_TL(chain, lb, ub, maxtime.count(), eps, true, true)); 106 | return true; 107 | } 108 | 109 | TRAC_IK_PUBLIC 110 | static double JointErr(const KDL::JntArray & arr1, const KDL::JntArray & arr2) 111 | { 112 | double err = 0; 113 | for (uint i = 0; i < arr1.data.size(); i++) { 114 | err += pow(arr1(i) - arr2(i), 2); 115 | } 116 | 117 | return err; 118 | } 119 | 120 | TRAC_IK_PUBLIC 121 | int CartToJnt( 122 | const KDL::JntArray & q_init, const KDL::Frame & p_in, KDL::JntArray & q_out, 123 | const KDL::Twist & bounds = KDL::Twist::Zero()); 124 | 125 | inline void SetSolveType(SolveType _type) 126 | { 127 | solvetype = _type; 128 | } 129 | 130 | inline SolveType GetSolveType() const 131 | { 132 | return solvetype; 133 | } 134 | 135 | private: 136 | bool initialized; 137 | KDL::Chain chain; 138 | KDL::JntArray lb, ub; 139 | std::unique_ptr jacsolver; 140 | double eps; 141 | std::chrono::duration maxtime; 142 | SolveType solvetype; 143 | 144 | std::unique_ptr nl_solver; 145 | std::unique_ptr iksolver; 146 | 147 | std::chrono::time_point> start_time; 148 | 149 | template 150 | bool runSolver( 151 | T1 & solver, T2 & other_solver, 152 | const KDL::JntArray & q_init, 153 | const KDL::Frame & p_in); 154 | 155 | bool runKDL(const KDL::JntArray & q_init, const KDL::Frame & p_in); 156 | bool runNLOPT(const KDL::JntArray & q_init, const KDL::Frame & p_in); 157 | 158 | void normalize_seed(const KDL::JntArray & seed, KDL::JntArray & solution); 159 | void normalize_limits(const KDL::JntArray & seed, KDL::JntArray & solution); 160 | 161 | std::vector types; 162 | 163 | std::mutex mtx_; 164 | std::vector solutions; 165 | std::vector> errors; 166 | 167 | std::thread task1, task2; 168 | KDL::Twist bounds; 169 | 170 | bool unique_solution(const KDL::JntArray & sol); 171 | 172 | inline static double fRand(double min, double max) 173 | { 174 | double f = static_cast(rand()) / RAND_MAX; // NOLINT 175 | return min + f * (max - min); 176 | } 177 | 178 | /* @brief Manipulation metrics and penalties taken from "Workspace 179 | Geometric Characterization and Manipulability of Industrial Robots", 180 | Ming-June, Tsia, PhD Thesis, Ohio State University, 1986. 181 | https://etd.ohiolink.edu/!etd.send_file?accession=osu1260297835 182 | */ 183 | double manipPenalty(const KDL::JntArray &); 184 | double ManipValue1(const KDL::JntArray &); 185 | double ManipValue2(const KDL::JntArray &); 186 | 187 | inline bool myEqual(const KDL::JntArray & a, const KDL::JntArray & b) 188 | { 189 | return (a.data - b.data).isZero(1e-4); 190 | } 191 | 192 | void initialize(); 193 | }; 194 | 195 | inline bool TRAC_IK::runKDL(const KDL::JntArray & q_init, const KDL::Frame & p_in) 196 | { 197 | return runSolver(*iksolver.get(), *nl_solver.get(), q_init, p_in); 198 | } 199 | 200 | inline bool TRAC_IK::runNLOPT(const KDL::JntArray & q_init, const KDL::Frame & p_in) 201 | { 202 | return runSolver(*nl_solver.get(), *iksolver.get(), q_init, p_in); 203 | } 204 | 205 | } // namespace TRAC_IK 206 | 207 | #endif // TRAC_IK__TRAC_IK_HPP_ 208 | -------------------------------------------------------------------------------- /trac_ik_lib/src/kdl_tl.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, TRACLabs, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | #include "trac_ik/kdl_tl.hpp" 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | namespace KDL 37 | { 38 | ChainIkSolverPos_TL::ChainIkSolverPos_TL( 39 | const Chain & _chain, const JntArray & _q_min, 40 | const JntArray & _q_max, double _maxtime, double _eps, 41 | bool _random_restart, bool _try_jl_wrap) 42 | : chain(_chain), q_min(_q_min), q_max(_q_max), vik_solver(chain), fksolver(chain), delta_q( 43 | chain.getNrOfJoints()), 44 | maxtime(std::chrono::duration(_maxtime)), eps(_eps), rr(_random_restart), wrap( 45 | _try_jl_wrap) 46 | { 47 | assert(chain.getNrOfJoints() == _q_min.data.size()); 48 | assert(chain.getNrOfJoints() == _q_max.data.size()); 49 | 50 | reset(); 51 | 52 | for (uint i = 0; i < chain.segments.size(); i++) { 53 | std::string type = chain.segments[i].getJoint().getTypeName(); 54 | if (type.find("Rot") != std::string::npos) { 55 | if (_q_max(types.size()) >= std::numeric_limits::max() && 56 | _q_min(types.size()) <= std::numeric_limits::lowest()) 57 | { 58 | types.push_back(KDL::BasicJointType::Continuous); 59 | } else {types.push_back(KDL::BasicJointType::RotJoint);} 60 | } else if (type.find("Trans") != std::string::npos) { 61 | types.push_back(KDL::BasicJointType::TransJoint); 62 | } 63 | } 64 | 65 | assert(types.size() == static_cast(_q_max.data.size())); // NOLINT 66 | } 67 | 68 | 69 | int ChainIkSolverPos_TL::CartToJnt( 70 | const KDL::JntArray & q_init, const KDL::Frame & p_in, 71 | KDL::JntArray & q_out, const KDL::Twist _bounds) 72 | { 73 | if (aborted) { 74 | return -3; 75 | } 76 | 77 | std::chrono::duration timediff; 78 | std::chrono::time_point> start_time( 79 | std::chrono::system_clock::now()); 80 | q_out = q_init; 81 | bounds = _bounds; 82 | 83 | do { 84 | fksolver.JntToCart(q_out, f); 85 | delta_twist = diffRelative(p_in, f); 86 | 87 | if (std::abs(delta_twist.vel.x()) <= std::abs(bounds.vel.x())) { 88 | delta_twist.vel.x(0); 89 | } 90 | 91 | if (std::abs(delta_twist.vel.y()) <= std::abs(bounds.vel.y())) { 92 | delta_twist.vel.y(0); 93 | } 94 | 95 | if (std::abs(delta_twist.vel.z()) <= std::abs(bounds.vel.z())) { 96 | delta_twist.vel.z(0); 97 | } 98 | 99 | if (std::abs(delta_twist.rot.x()) <= std::abs(bounds.rot.x())) { 100 | delta_twist.rot.x(0); 101 | } 102 | 103 | if (std::abs(delta_twist.rot.y()) <= std::abs(bounds.rot.y())) { 104 | delta_twist.rot.y(0); 105 | } 106 | 107 | if (std::abs(delta_twist.rot.z()) <= std::abs(bounds.rot.z())) { 108 | delta_twist.rot.z(0); 109 | } 110 | 111 | if (Equal(delta_twist, Twist::Zero(), eps)) { 112 | return 1; 113 | } 114 | 115 | delta_twist = diff(f, p_in); 116 | 117 | vik_solver.CartToJnt(q_out, delta_twist, delta_q); 118 | KDL::JntArray q_curr; 119 | 120 | Add(q_out, delta_q, q_curr); 121 | 122 | for (unsigned int j = 0; j < q_min.data.size(); j++) { 123 | if (types[j] == KDL::BasicJointType::Continuous) { 124 | continue; 125 | } 126 | if (q_curr(j) < q_min(j)) { 127 | if (!wrap || types[j] == KDL::BasicJointType::TransJoint) { 128 | // KDL's default 129 | q_curr(j) = q_min(j); 130 | } else { 131 | // Find actual wrapped angle between limit and joint 132 | double diffangle = fmod(q_min(j) - q_curr(j), 2 * M_PI); 133 | // Subtract that angle from limit and go into the range by a 134 | // revolution 135 | double curr_angle = q_min(j) - diffangle + 2 * M_PI; 136 | if (curr_angle > q_max(j)) { 137 | q_curr(j) = q_min(j); 138 | } else { 139 | q_curr(j) = curr_angle; 140 | } 141 | } 142 | } 143 | } 144 | 145 | for (unsigned int j = 0; j < q_max.data.size(); j++) { 146 | if (types[j] == KDL::BasicJointType::Continuous) { 147 | continue; 148 | } 149 | 150 | if (q_curr(j) > q_max(j)) { 151 | if (!wrap || types[j] == KDL::BasicJointType::TransJoint) { 152 | // KDL's default 153 | q_curr(j) = q_max(j); 154 | } else { 155 | // Find actual wrapped angle between limit and joint 156 | double diffangle = fmod(q_curr(j) - q_max(j), 2 * M_PI); 157 | // Add that angle to limit and go into the range by a revolution 158 | double curr_angle = q_max(j) + diffangle - 2 * M_PI; 159 | if (curr_angle < q_min(j)) { 160 | q_curr(j) = q_max(j); 161 | } else { 162 | q_curr(j) = curr_angle; 163 | } 164 | } 165 | } 166 | } 167 | 168 | Subtract(q_out, q_curr, q_out); 169 | 170 | if (q_out.data.isZero(std::numeric_limits::epsilon())) { 171 | if (rr) { 172 | for (unsigned int j = 0; j < q_out.data.size(); j++) { 173 | if (types[j] == KDL::BasicJointType::Continuous) { 174 | q_curr(j) = fRand(q_curr(j) - 2 * M_PI, q_curr(j) + 2 * M_PI); 175 | } else { 176 | q_curr(j) = fRand(q_min(j), q_max(j)); 177 | } 178 | } 179 | } 180 | 181 | // Below would be an optimization to the normal KDL, where when it 182 | // gets stuck, it returns immediately. Don't use to compare KDL with 183 | // random restarts or TRAC-IK to plain KDL. 184 | 185 | // else { 186 | // q_out=q_curr; 187 | // return -3; 188 | // } 189 | } 190 | 191 | q_out = q_curr; 192 | 193 | timediff = std::chrono::system_clock::now() - start_time; 194 | } while (timediff < maxtime && !aborted); 195 | 196 | return -3; 197 | } 198 | 199 | ChainIkSolverPos_TL::~ChainIkSolverPos_TL() 200 | { 201 | } 202 | 203 | 204 | } // namespace KDL 205 | -------------------------------------------------------------------------------- /trac_ik_lib/include/trac_ik/dual_quaternion.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, TRACLabs, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | #ifndef TRAC_IK__DUAL_QUATERNION_H_ 31 | #define TRAC_IK__DUAL_QUATERNION_H_ 32 | 33 | #include "trac_ik/math3d.h" 34 | 35 | using math3d::point3d; 36 | using math3d::matrix; 37 | using math3d::quaternion; 38 | 39 | 40 | template 41 | inline int sign(T v) 42 | { 43 | return (v < 0) ? -1 : 1; 44 | } 45 | 46 | void set_quaternion_matrix( 47 | matrix & M, const quaternion & q, int i = 0, 48 | int j = 0, double w = 1.0) 49 | { 50 | // {{a, -b, -c, -d}, {b, a, -d, c}, {c, d, a, -b}, {d, -c, b, a}} 51 | M(i, j) = w * q.w; 52 | M(i, j + 1) = -w * q.i; 53 | M(i, j + 2) = -w * q.j; 54 | M(i, j + 3) = -w * q.k; 55 | M(i + 1, j) = w * q.i; 56 | M(i + 1, j + 1) = w * q.w; 57 | M(i + 1, j + 2) = -w * q.k; 58 | M(i + 1, j + 3) = w * q.j; 59 | M(i + 2, j) = w * q.j; 60 | M(i + 2, j + 1) = w * q.k; 61 | M(i + 2, j + 2) = w * q.w; 62 | M(i + 2, j + 3) = -w * q.i; 63 | M(i + 3, j) = w * q.k; 64 | M(i + 3, j + 1) = -w * q.j; 65 | M(i + 3, j + 2) = w * q.i; 66 | M(i + 3, j + 3) = w * q.w; 67 | } 68 | 69 | struct dual_quaternion 70 | { 71 | quaternion R, tR_2; 72 | 73 | explicit dual_quaternion(double v = 1.0) 74 | : R(v), tR_2(0) 75 | { 76 | } 77 | 78 | static constexpr double dq_epsilon = 1e-8; 79 | 80 | static dual_quaternion rigid_transformation(const quaternion & r, const point3d & t) 81 | { 82 | dual_quaternion result; 83 | result.R = r; 84 | result.tR_2 = (quaternion::convert(t) * r) *= 0.5; 85 | return result; 86 | } 87 | static dual_quaternion convert(const double * p) 88 | { 89 | dual_quaternion result; 90 | result.R = quaternion::convert(p); 91 | result.tR_2 = quaternion::convert(p + 4); 92 | return result; 93 | } 94 | 95 | dual_quaternion & normalize() 96 | { 97 | double n = norm(R) * sign(R.w); 98 | R *= 1.0 / n; 99 | tR_2 *= 1.0 / n; 100 | double d = dot(R, tR_2); 101 | // tR_2 += (-d)*R; 102 | quaternion r2 = R; 103 | r2 *= -d; 104 | tR_2 += r2; 105 | return *this; 106 | } 107 | 108 | point3d get_translation() 109 | { 110 | quaternion t = tR_2 * ~R; 111 | point3d result; 112 | result.x = 2 * t.i; 113 | result.y = 2 * t.j; 114 | result.z = 2 * t.k; 115 | return result; 116 | } 117 | 118 | void to_vector(double * p) 119 | { 120 | R.to_vector(p); 121 | tR_2.to_vector(p + 4); 122 | } 123 | 124 | dual_quaternion & operator+=(const dual_quaternion & a) 125 | { 126 | R += a.R; 127 | tR_2 += a.tR_2; 128 | return *this; 129 | } 130 | 131 | dual_quaternion & operator*=(double a) 132 | { 133 | R *= a; 134 | tR_2 *= a; 135 | return *this; 136 | } 137 | 138 | dual_quaternion & log() // computes log map tangent at identity 139 | { 140 | // assumes qual_quaternion is unitary 141 | const double h0 = std::acos(R.w); 142 | if (h0 * h0 < dq_epsilon) { // small angle approximation: sin(h0)=h0, cos(h0)=1 143 | R.w = 0.0; 144 | R *= 0.5; 145 | tR_2.w = 0.0; 146 | tR_2 *= 0.5; 147 | } else { 148 | R.w = 0.0; 149 | const double ish0 = 1.0 / norm(R); 150 | // R *= ish0; 151 | math3d::normalize(R); // R=s0 152 | const double he = -tR_2.w * ish0; 153 | tR_2.w = 0.0; 154 | 155 | quaternion Rp(R); 156 | Rp *= -dot(R, tR_2) / dot(R, R); 157 | tR_2 += Rp; 158 | tR_2 *= ish0; // tR_2=se 159 | 160 | tR_2 *= h0; 161 | Rp = R; 162 | Rp *= he; 163 | tR_2 += Rp; 164 | tR_2 *= 0.5; 165 | R *= h0 * 0.5; 166 | } 167 | 168 | return *this; 169 | } 170 | 171 | dual_quaternion & exp() // computes exp map tangent at identity 172 | { 173 | // assumes qual_quaternion is on tangent space 174 | const double h0 = 2.0 * norm(R); 175 | 176 | if (h0 * h0 < dq_epsilon) { // small angle approximation: sin(h0)=h0, cos(h0)=1 177 | R *= 2.0; 178 | R.w = 1.0; 179 | tR_2 *= 2.0; 180 | // normalize(); 181 | } else { 182 | const double he = 4.0 * math3d::dot(tR_2, R) / h0; 183 | const double sh0 = sin(h0), ch0 = cos(h0); 184 | quaternion Rp(R); 185 | Rp *= -dot(R, tR_2) / dot(R, R); 186 | tR_2 += Rp; 187 | tR_2 *= 2.0 / h0; // tR_2=se 188 | 189 | 190 | tR_2 *= sh0; 191 | Rp = R; 192 | Rp *= he * ch0 * 2.0 / h0; 193 | tR_2 += Rp; 194 | tR_2.w = -he * sh0; 195 | 196 | R *= sh0 * 2.0 / h0; 197 | R.w = ch0; 198 | } 199 | normalize(); 200 | return *this; 201 | } 202 | }; 203 | 204 | 205 | dual_quaternion operator*(const dual_quaternion & a, const dual_quaternion & b) 206 | { 207 | dual_quaternion result; 208 | result.R = a.R * b.R; 209 | result.tR_2 = a.R * b.tR_2 + a.tR_2 * b.R; 210 | return result; 211 | } 212 | 213 | dual_quaternion operator~(const dual_quaternion & a) 214 | { 215 | dual_quaternion result; 216 | result.R = ~a.R; 217 | result.tR_2 = ((~a.tR_2) *= -1); 218 | return result; 219 | } 220 | 221 | dual_quaternion operator!(const dual_quaternion & a) 222 | { 223 | dual_quaternion result; 224 | result.R = ~a.R; 225 | result.tR_2 = ~a.tR_2; 226 | return result; 227 | } 228 | 229 | double dot(const dual_quaternion & a, const dual_quaternion & b) 230 | { 231 | return dot(a.R, b.R) + dot(a.tR_2, b.tR_2); 232 | } 233 | 234 | void set_dual_quaternion_matrix( 235 | matrix & M, const dual_quaternion & dq, int i = 0, 236 | int j = 0, double w = 1.0) 237 | { 238 | set_quaternion_matrix(M, dq.R, i, j, w); 239 | M(i, j + 4) = M(i, j + 5) = M(i, j + 6) = M(i, j + 7) = 0; 240 | M(i + 1, j + 4) = M(i + 1, j + 5) = M(i + 1, j + 6) = M(i + 1, j + 7) = 0; 241 | M(i + 2, j + 4) = M(i + 2, j + 5) = M(i + 2, j + 6) = M(i + 2, j + 7) = 0; 242 | M(i + 3, j + 4) = M(i + 3, j + 5) = M(i + 3, j + 6) = M(i + 3, j + 7) = 0; 243 | set_quaternion_matrix(M, dq.tR_2, i + 4, j, w); 244 | set_quaternion_matrix(M, dq.R, i + 4, j + 4, w); 245 | } 246 | 247 | dual_quaternion log(dual_quaternion a) 248 | { 249 | return a.log(); 250 | } 251 | dual_quaternion exp(dual_quaternion a) 252 | { 253 | return a.exp(); 254 | } 255 | 256 | 257 | std::ostream & operator<<(std::ostream & out, const dual_quaternion & dq) 258 | { 259 | return out << "( " << dq.R.w << ", " << dq.R.i << ", " << dq.R.j << ", " << dq.R.k << ", " << 260 | dq.tR_2.w << ", " << dq.tR_2.i << ", " << dq.tR_2.j << ", " << dq.tR_2.k << " )"; 261 | } 262 | 263 | #endif // TRAC_IK__DUAL_QUATERNION_H_ 264 | -------------------------------------------------------------------------------- /trac_ik_examples/src/ik_tests.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, TRACLabs, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include "kdl/chainiksolverpos_nr_jl.hpp" 37 | #include "rclcpp/rclcpp.hpp" 38 | #include "trac_ik/trac_ik.hpp" 39 | 40 | void test( 41 | const rclcpp::Node::SharedPtr node, double num_samples, std::string chain_start, 42 | std::string chain_end, double timeout, std::string urdf_xml) 43 | { 44 | double eps = 1e-5; 45 | 46 | // This constructor parses the URDF loaded in rosparm urdf_xml into the 47 | // needed KDL structures. We then pull these out to compare against the KDL 48 | // IK solver. 49 | TRAC_IK::TRAC_IK tracik_solver(chain_start, chain_end, urdf_xml, timeout, eps); 50 | 51 | KDL::Chain chain; 52 | KDL::JntArray ll, ul; // lower joint limits, upper joint limits 53 | 54 | bool valid = tracik_solver.getKDLChain(chain); 55 | 56 | if (!valid) { 57 | RCLCPP_ERROR(node->get_logger(), "There was no valid KDL chain found"); 58 | return; 59 | } 60 | 61 | valid = tracik_solver.getKDLLimits(ll, ul); 62 | 63 | if (!valid) { 64 | RCLCPP_ERROR(node->get_logger(), "There were no valid KDL joint limits found"); 65 | return; 66 | } 67 | 68 | assert(chain.getNrOfJoints() == ll.data.size()); 69 | assert(chain.getNrOfJoints() == ul.data.size()); 70 | 71 | RCLCPP_INFO(node->get_logger(), "Using %d joints", chain.getNrOfJoints()); 72 | 73 | 74 | // Set up KDL IK 75 | KDL::ChainFkSolverPos_recursive fk_solver(chain); // Forward kin. solver 76 | KDL::ChainIkSolverVel_pinv vik_solver(chain); // PseudoInverse vel solver 77 | // Joint Limit Solver 78 | KDL::ChainIkSolverPos_NR_JL kdl_solver(chain, ll, ul, fk_solver, vik_solver, 1, eps); 79 | // 1 iteration per solve (will wrap in timed loop to compare with TRAC-IK) 80 | 81 | 82 | // Create Nominal chain configuration midway between all joint limits 83 | KDL::JntArray nominal(chain.getNrOfJoints()); 84 | 85 | for (uint j = 0; j < nominal.data.size(); j++) { 86 | nominal(j) = (ll(j) + ul(j)) / 2.0; 87 | } 88 | 89 | // Create desired number of valid, random joint configurations 90 | std::vector JointList; 91 | KDL::JntArray q(chain.getNrOfJoints()); 92 | 93 | std::random_device rd; 94 | std::mt19937 gen(rd()); 95 | for (uint i = 0; i < num_samples; i++) { 96 | for (uint j = 0; j < ll.data.size(); j++) { 97 | std::uniform_real_distribution dist(ll(j), ul(j)); 98 | q(j) = dist(gen); 99 | } 100 | JointList.push_back(q); 101 | } 102 | 103 | 104 | std::chrono::time_point> start_time; 105 | std::chrono::duration diff; 106 | 107 | KDL::JntArray result; 108 | KDL::Frame end_effector_pose; 109 | int rc; 110 | 111 | double total_time = 0; 112 | uint success = 0; 113 | 114 | RCLCPP_INFO_STREAM( 115 | node->get_logger(), 116 | "*** Testing KDL with " << num_samples << " random samples"); 117 | 118 | for (uint i = 0; i < num_samples; i++) { 119 | fk_solver.JntToCart(JointList[i], end_effector_pose); 120 | result = nominal; // start with nominal 121 | start_time = std::chrono::system_clock::now(); 122 | do { 123 | q = result; // when iterating start with last solution 124 | rc = kdl_solver.CartToJnt(q, end_effector_pose, result); 125 | diff = std::chrono::system_clock::now() - start_time; 126 | } while (rc < 0 && diff.count() < timeout); 127 | total_time += diff.count(); 128 | if (rc >= 0) { 129 | success++; 130 | } 131 | 132 | if (static_cast(static_cast(i) / num_samples * 100) % 10 == 0) { 133 | RCLCPP_INFO_STREAM_THROTTLE( 134 | node->get_logger(), 135 | *(node->get_clock()), 136 | 1.0, 137 | static_cast((i) / num_samples * 100) << "% done"); 138 | } 139 | } 140 | 141 | RCLCPP_INFO_STREAM( 142 | node->get_logger(), 143 | "KDL found " << success << " solutions (" << 100.0 * success / num_samples << 144 | "%) with an average of " << total_time / num_samples << 145 | " secs per sample"); 146 | 147 | 148 | total_time = 0; 149 | success = 0; 150 | 151 | RCLCPP_INFO_STREAM( 152 | node->get_logger(), "*** Testing TRAC-IK with " << num_samples << " random samples"); 153 | 154 | for (uint i = 0; i < num_samples; i++) { 155 | fk_solver.JntToCart(JointList[i], end_effector_pose); 156 | start_time = std::chrono::system_clock::now(); 157 | rc = tracik_solver.CartToJnt(nominal, end_effector_pose, result); 158 | diff = std::chrono::system_clock::now() - start_time; 159 | total_time += diff.count(); 160 | if (rc >= 0) { 161 | success++; 162 | } 163 | 164 | if (static_cast(static_cast(i) / num_samples * 100) % 10 == 0) { 165 | RCLCPP_INFO_STREAM_THROTTLE( 166 | node->get_logger(), 167 | *(node->get_clock()), 168 | 1.0, 169 | static_cast((i) / num_samples * 100) << "% done"); 170 | } 171 | } 172 | 173 | RCLCPP_INFO_STREAM( 174 | node->get_logger(), 175 | "TRAC-IK found " << success << " solutions (" << 100.0 * success / num_samples << 176 | "%) with an average of " << total_time / num_samples << 177 | " secs per sample"); 178 | } 179 | 180 | 181 | int main(int argc, char ** argv) 182 | { 183 | rclcpp::init(argc, argv); 184 | auto node = rclcpp::Node::make_shared("ik_tests"); 185 | 186 | int num_samples; 187 | std::string chain_start, chain_end, urdf_xml; 188 | double timeout; 189 | 190 | node->declare_parameter("num_samples", 1000); 191 | node->declare_parameter("timeout", 0.005); 192 | node->declare_parameters( 193 | std::string(), // parameters are not namespaced 194 | std::map{ 195 | {"chain_start", std::string()}, 196 | {"chain_end", std::string()}, 197 | {"robot_description", std::string()}, 198 | }); 199 | 200 | node->get_parameter("num_samples", num_samples); 201 | node->get_parameter("timeout", timeout); 202 | node->get_parameter("chain_start", chain_start); 203 | node->get_parameter("chain_end", chain_end); 204 | node->get_parameter("robot_description", urdf_xml); 205 | 206 | if (chain_start.empty() || chain_end.empty()) { 207 | RCLCPP_FATAL(node->get_logger(), "Missing chain info in launch file"); 208 | exit(-1); 209 | } 210 | 211 | if (num_samples < 1) { 212 | num_samples = 1; 213 | } 214 | 215 | test(node, num_samples, chain_start, chain_end, timeout, urdf_xml); 216 | 217 | // Useful when a script loops over multiple launch files testing different robot chains 218 | // std::vector commandVector; 219 | // commandVector.push_back((char*)"killall"); 220 | // commandVector.push_back((char*)"-9"); 221 | // commandVector.push_back((char*)"roslaunch"); 222 | // commandVector.push_back(NULL); 223 | 224 | // char **command = &commandVector[0]; 225 | // execvp(command[0],command); 226 | 227 | return 0; 228 | } 229 | -------------------------------------------------------------------------------- /trac_ik_kinematics_plugin/include/trac_ik/trac_ik_kinematics_plugin.hpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************** 2 | Copyright (c) 2015, TRACLabs, Inc. 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without modification, 6 | are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, 9 | this list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software 17 | without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 22 | IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 23 | INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 24 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 25 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 26 | LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 28 | OF THE POSSIBILITY OF SUCH DAMAGE. 29 | ********************************************************************************/ 30 | 31 | #ifndef TRAC_IK_KINEMATICS_PLUGIN_ 32 | #define TRAC_IK_KINEMATICS_PLUGIN_ 33 | 34 | #include 35 | #include 36 | 37 | namespace trac_ik_kinematics_plugin 38 | { 39 | 40 | class TRAC_IKKinematicsPlugin : public kinematics::KinematicsBase 41 | { 42 | std::vector joint_names_; 43 | std::vector link_names_; 44 | 45 | uint num_joints_; 46 | bool active_; // Internal variable that indicates whether solvers are configured and ready 47 | 48 | KDL::Chain chain; 49 | bool position_ik_; 50 | 51 | KDL::JntArray joint_min, joint_max; 52 | 53 | std::string solve_type; 54 | 55 | public: 56 | const std::vector& getJointNames() const 57 | { 58 | return joint_names_; 59 | } 60 | const std::vector& getLinkNames() const 61 | { 62 | return link_names_; 63 | } 64 | 65 | 66 | /** @class 67 | * @brief Interface for an TRAC-IK kinematics plugin 68 | */ 69 | TRAC_IKKinematicsPlugin(): active_(false), position_ik_(false) {} 70 | 71 | ~TRAC_IKKinematicsPlugin() 72 | { 73 | } 74 | 75 | /** 76 | * @brief Given a desired pose of the end-effector, compute the joint angles to reach it 77 | * @param ik_pose the desired pose of the link 78 | * @param ik_seed_state an initial guess solution for the inverse kinematics 79 | * @param solution the solution vector 80 | * @param error_code an error code that encodes the reason for failure or success 81 | * @return True if a valid solution was found, false otherwise 82 | */ 83 | 84 | // Returns the first IK solution that is within joint limits, this is called by get_ik() service 85 | bool getPositionIK(const geometry_msgs::Pose &ik_pose, 86 | const std::vector &ik_seed_state, 87 | std::vector &solution, 88 | moveit_msgs::MoveItErrorCodes &error_code, 89 | const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; 90 | 91 | /** 92 | * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. 93 | * This particular method is intended for "searching" for a solutions by stepping through the redundancy 94 | * (or other numerical routines). 95 | * @param ik_pose the desired pose of the link 96 | * @param ik_seed_state an initial guess solution for the inverse kinematics 97 | * @return True if a valid solution was found, false otherwise 98 | */ 99 | bool searchPositionIK(const geometry_msgs::Pose &ik_pose, 100 | const std::vector &ik_seed_state, 101 | double timeout, 102 | std::vector &solution, 103 | moveit_msgs::MoveItErrorCodes &error_code, 104 | const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; 105 | 106 | /** 107 | * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. 108 | * This particular method is intended for "searching" for a solutions by stepping through the redundancy 109 | * (or other numerical routines). 110 | * @param ik_pose the desired pose of the link 111 | * @param ik_seed_state an initial guess solution for the inverse kinematics 112 | * @param the distance that the redundancy can be from the current position 113 | * @return True if a valid solution was found, false otherwise 114 | */ 115 | bool searchPositionIK(const geometry_msgs::Pose &ik_pose, 116 | const std::vector &ik_seed_state, 117 | double timeout, 118 | const std::vector &consistency_limits, 119 | std::vector &solution, 120 | moveit_msgs::MoveItErrorCodes &error_code, 121 | const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; 122 | 123 | /** 124 | * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. 125 | * This particular method is intended for "searching" for a solutions by stepping through the redundancy 126 | * (or other numerical routines). 127 | * @param ik_pose the desired pose of the link 128 | * @param ik_seed_state an initial guess solution for the inverse kinematics 129 | * @return True if a valid solution was found, false otherwise 130 | */ 131 | bool searchPositionIK(const geometry_msgs::Pose &ik_pose, 132 | const std::vector &ik_seed_state, 133 | double timeout, 134 | std::vector &solution, 135 | const IKCallbackFn &solution_callback, 136 | moveit_msgs::MoveItErrorCodes &error_code, 137 | const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; 138 | 139 | /** 140 | * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. 141 | * This particular method is intended for "searching" for a solutions by stepping through the redundancy 142 | * (or other numerical routines). The consistency_limit specifies that only certain redundancy positions 143 | * around those specified in the seed state are admissible and need to be searched. 144 | * @param ik_pose the desired pose of the link 145 | * @param ik_seed_state an initial guess solution for the inverse kinematics 146 | * @param consistency_limit the distance that the redundancy can be from the current position 147 | * @return True if a valid solution was found, false otherwise 148 | */ 149 | bool searchPositionIK(const geometry_msgs::Pose &ik_pose, 150 | const std::vector &ik_seed_state, 151 | double timeout, 152 | const std::vector &consistency_limits, 153 | std::vector &solution, 154 | const IKCallbackFn &solution_callback, 155 | moveit_msgs::MoveItErrorCodes &error_code, 156 | const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; 157 | 158 | bool searchPositionIK(const geometry_msgs::Pose &ik_pose, 159 | const std::vector &ik_seed_state, 160 | double timeout, 161 | std::vector &solution, 162 | const IKCallbackFn &solution_callback, 163 | moveit_msgs::MoveItErrorCodes &error_code, 164 | const std::vector &consistency_limits, 165 | const kinematics::KinematicsQueryOptions &options) const; 166 | 167 | 168 | /** 169 | * @brief Given a set of joint angles and a set of links, compute their pose 170 | * 171 | * This FK routine is only used if 'use_plugin_fk' is set in the 'arm_kinematics_constraint_aware' node, 172 | * otherwise ROS TF is used to calculate the forward kinematics 173 | * 174 | * @param link_names A set of links for which FK needs to be computed 175 | * @param joint_angles The state for which FK is being computed 176 | * @param poses The resultant set of poses (in the frame returned by getBaseFrame()) 177 | * @return True if a valid solution was found, false otherwise 178 | */ 179 | bool getPositionFK(const std::vector &link_names, 180 | const std::vector &joint_angles, 181 | std::vector &poses) const; 182 | 183 | 184 | bool initialize(const std::string &robot_description, 185 | const std::string& group_name, 186 | const std::string& base_name, 187 | const std::string& tip_name, 188 | double search_discretization); 189 | 190 | private: 191 | 192 | int getKDLSegmentIndex(const std::string &name) const; 193 | 194 | }; // end class 195 | } 196 | 197 | #endif 198 | -------------------------------------------------------------------------------- /trac_ik_python/swig/trac_ik_wrap.i: -------------------------------------------------------------------------------- 1 | /* trac_ik_wrap.i */ 2 | %module trac_ik_wrap 3 | 4 | // Author: Sammy Pfeiffer 5 | 6 | %{ 7 | /* Includes the header in the wrapper code */ 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | %} 15 | 16 | // We need this or we will get on runtime 17 | // NotImplementedError: Wrong number or type of arguments for overloaded function 18 | %include 19 | %include 20 | 21 | // From http://stackoverflow.com/a/8752983 22 | // Instantiate templates used by example 23 | namespace std { 24 | %template(IntVector) vector; 25 | %template(DoubleVector) vector; 26 | %template(StringVector) vector; 27 | %template(ConstCharVector) vector; 28 | } 29 | 30 | // USEFUL DOCS: http://www.swig.org/Doc1.3/SWIG.html 31 | 32 | 33 | // Ignore original constructors as they are not useful in Python 34 | // Note the full namespacing 35 | %ignore TRAC_IK::TRAC_IK::TRAC_IK(const KDL::Chain& _chain, const KDL::JntArray& _q_min, const KDL::JntArray& _q_max, double _maxtime=0.005, double _eps=1e-5, SolveType _type=Speed); 36 | %ignore TRAC_IK::TRAC_IK::TRAC_IK(const std::string& base_link, const std::string& tip_link, const std::string& URDF_param="/robot_description", double _maxtime=0.005, double _eps=1e-5, SolveType _type=Speed); 37 | 38 | // Ignore the runKDL and runNLOPT methods as they fail to be wrapped (and are private anyways) 39 | %ignore TRAC_IK::runKDL(const KDL::JntArray &q_init, const KDL::Frame &p_in); 40 | %ignore TRAC_IK::runNLOPT(const KDL::JntArray &q_init, const KDL::Frame &p_in); 41 | 42 | // Ignore other methods that we will wrap in a more usable way 43 | %ignore TRAC_IK::getKDLLimits(KDL::JntArray& lb_, KDL::JntArray& ub_); 44 | %ignore TRAC_IK::setKDLLimits(KDL::JntArray& lb_, KDL::JntArray& ub_); 45 | 46 | // All variables will use const reference typemaps 47 | // This eases dealing with std::vectors 48 | %naturalvar; 49 | 50 | // Parse the original header file to generate wrappers 51 | %include 52 | 53 | // Create a more Python friendly constructor 54 | %extend TRAC_IK::TRAC_IK { 55 | // Based on trac_ik_kinematics_plugin.cpp implementation 56 | // As we can't access the private variables of the TRAC_IK class from this extension 57 | // this is the only way I found to make another constructor 58 | // thanks to: http://stackoverflow.com/questions/33564645/how-to-add-an-alternative-constructor-to-the-target-language-specifically-pytho 59 | TRAC_IK(const std::string& base_link, const std::string& tip_link, const std::string& urdf_string, 60 | double timeout, double epsilon, const std::string& solve_type="Speed"){ 61 | 62 | urdf::Model robot_model; 63 | 64 | robot_model.initString(urdf_string); 65 | 66 | ROS_DEBUG_STREAM_NAMED("trac_ik","Reading joints and links from URDF"); 67 | 68 | KDL::Tree tree; 69 | 70 | if (!kdl_parser::treeFromUrdfModel(robot_model, tree)) { 71 | ROS_FATAL("Failed to extract kdl tree from xml robot description"); 72 | } 73 | 74 | 75 | KDL::Chain chain; 76 | 77 | if(!tree.getChain(base_link, tip_link, chain)) { 78 | ROS_FATAL("Couldn't find chain %s to %s",base_link.c_str(),tip_link.c_str()); 79 | } 80 | 81 | uint num_joints_; 82 | num_joints_ = chain.getNrOfJoints(); 83 | 84 | std::vector chain_segs = chain.segments; 85 | 86 | urdf::JointConstSharedPtr joint; 87 | 88 | std::vector l_bounds, u_bounds; 89 | 90 | KDL::JntArray joint_min, joint_max; 91 | 92 | joint_min.resize(num_joints_); 93 | joint_max.resize(num_joints_); 94 | 95 | std::vector link_names_; 96 | std::vector joint_names_; 97 | 98 | uint joint_num=0; 99 | for(unsigned int i = 0; i < chain_segs.size(); ++i) { 100 | 101 | link_names_.push_back(chain_segs[i].getName()); 102 | joint = robot_model.getJoint(chain_segs[i].getJoint().getName()); 103 | if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) { 104 | joint_num++; 105 | assert(joint_num<=num_joints_); 106 | float lower, upper; 107 | int hasLimits; 108 | joint_names_.push_back(joint->name); 109 | if ( joint->type != urdf::Joint::CONTINUOUS ) { 110 | if(joint->safety) { 111 | lower = std::max(joint->limits->lower, joint->safety->soft_lower_limit); 112 | upper = std::min(joint->limits->upper, joint->safety->soft_upper_limit); 113 | } else { 114 | lower = joint->limits->lower; 115 | upper = joint->limits->upper; 116 | } 117 | hasLimits = 1; 118 | } 119 | else { 120 | hasLimits = 0; 121 | } 122 | if(hasLimits) { 123 | joint_min(joint_num-1)=lower; 124 | joint_max(joint_num-1)=upper; 125 | } 126 | else { 127 | joint_min(joint_num-1)=std::numeric_limits::lowest(); 128 | joint_max(joint_num-1)=std::numeric_limits::max(); 129 | } 130 | ROS_DEBUG_STREAM("IK Using joint "< CartToJnt(const std::vector q_init, 161 | const double x, const double y, const double z, 162 | const double rx, const double ry, const double rz, const double rw, 163 | // bounds x y z 164 | const double boundx=0.0, const double boundy=0.0, const double boundz=0.0, 165 | // bounds on rotation x y z 166 | const double boundrx=0.0, const double boundry=0.0, const double boundrz=0.0) 167 | { 168 | 169 | KDL::Frame frame; 170 | geometry_msgs::Pose pose; 171 | pose.position.x = x; 172 | pose.position.y = y; 173 | pose.position.z = z; 174 | pose.orientation.x = rx; 175 | pose.orientation.y = ry; 176 | pose.orientation.z = rz; 177 | pose.orientation.w = rw; 178 | 179 | tf::poseMsgToKDL(pose, frame); 180 | 181 | KDL::JntArray in(q_init.size()), out(q_init.size()); 182 | 183 | for (uint z=0; z < q_init.size(); z++) 184 | in(z) = q_init[z]; 185 | 186 | KDL::Twist bounds = KDL::Twist::Zero(); 187 | bounds.vel.x(boundx); 188 | bounds.vel.y(boundy); 189 | bounds.vel.z(boundz); 190 | bounds.rot.x(boundrx); 191 | bounds.rot.y(boundry); 192 | bounds.rot.z(boundrz); 193 | 194 | int rc = $self->CartToJnt(in, frame, out, bounds); 195 | std::vector vout; 196 | // If no solution, return empty vector which acts as None 197 | if (rc == -3) 198 | return vout; 199 | 200 | for (uint z=0; z < q_init.size(); z++) 201 | vout.push_back(out(z)); 202 | 203 | return vout; 204 | } 205 | 206 | // Convenience method to check that calls to IK have the correct 207 | // number of qinit elements 208 | int getNrOfJointsInChain(){ 209 | KDL::Chain chain; 210 | $self->getKDLChain(chain); 211 | return (int) chain.getNrOfJoints(); 212 | } 213 | 214 | // Convenience method to get the list of joint names as used internally 215 | std::vector getJointNamesInChain(const std::string& urdf_string){ 216 | KDL::Chain chain; 217 | $self->getKDLChain(chain); 218 | std::vector chain_segs = chain.segments; 219 | 220 | std::vector joint_names_; 221 | std::vector link_names_; 222 | urdf::JointConstSharedPtr joint; 223 | 224 | urdf::Model robot_model; 225 | robot_model.initString(urdf_string); 226 | 227 | for(unsigned int i = 0; i < chain_segs.size(); ++i) { 228 | link_names_.push_back(chain_segs[i].getName()); 229 | joint = robot_model.getJoint(chain_segs[i].getJoint().getName()); 230 | if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) { 231 | joint_names_.push_back(joint->name); 232 | } 233 | } 234 | return joint_names_; 235 | } 236 | 237 | 238 | // Convenience method to get the list of link names as used internally 239 | std::vector getLinkNamesInChain(){ 240 | KDL::Chain chain; 241 | $self->getKDLChain(chain); 242 | std::vector chain_segs = chain.segments; 243 | std::vector link_names_; 244 | for(unsigned int i = 0; i < chain_segs.size(); ++i) { 245 | link_names_.push_back(chain_segs[i].getName()); 246 | } 247 | return link_names_; 248 | } 249 | 250 | 251 | // Get KDL limits 252 | std::vector getLowerBoundLimits(){ 253 | KDL::JntArray lb_; 254 | KDL::JntArray ub_; 255 | std::vector lb; 256 | $self->getKDLLimits(lb_, ub_); 257 | for(unsigned int i=0; i < lb_.rows(); i++){ 258 | lb.push_back(lb_(i)); 259 | } 260 | return lb; 261 | } 262 | 263 | std::vector getUpperBoundLimits(){ 264 | KDL::JntArray lb_; 265 | KDL::JntArray ub_; 266 | std::vector ub; 267 | $self->getKDLLimits(lb_, ub_); 268 | for(unsigned int i=0; i < ub_.rows(); i++){ 269 | ub.push_back(ub_(i)); 270 | } 271 | return ub; 272 | } 273 | 274 | 275 | // Set KDL limits, Python takes care of checking number of limits 276 | void setKDLLimits(const std::vector lb, const std::vector ub) { 277 | KDL::JntArray lb_; 278 | KDL::JntArray ub_; 279 | lb_.resize(lb.size()); 280 | for(unsigned int i=0; i < lb.size(); i++){ 281 | lb_(i) = lb[i]; 282 | } 283 | ub_.resize(ub.size()); 284 | for(unsigned int i=0; i < ub.size(); i++){ 285 | ub_(i) = ub[i]; 286 | } 287 | $self->setKDLLimits(lb_, ub_); 288 | } 289 | 290 | }; 291 | 292 | -------------------------------------------------------------------------------- /trac_ik_lib/src/trac_ik.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, TRACLabs, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | #include "trac_ik/trac_ik.hpp" 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include "Eigen/Geometry" 40 | #include "kdl_parser/kdl_parser.hpp" 41 | #include "rclcpp/rclcpp.hpp" 42 | #include "urdf/model.h" 43 | 44 | namespace TRAC_IK 45 | { 46 | static const rclcpp::Logger LOGGER = rclcpp::get_logger("trac_ik.ros.trac_ik"); 47 | 48 | TRAC_IK::TRAC_IK( 49 | const std::string & base_link, const std::string & tip_link, 50 | const std::string & urdf_xml, double _maxtime, double _eps, SolveType _type) 51 | : initialized(false), 52 | eps(_eps), 53 | maxtime(std::chrono::duration(_maxtime)), 54 | solvetype(_type) 55 | { 56 | urdf::Model robot_model; 57 | if (!robot_model.initString(urdf_xml)) { 58 | RCLCPP_FATAL(LOGGER, "Unable to initialize urdf::Model from robot description."); 59 | } 60 | 61 | RCLCPP_DEBUG(LOGGER, "Reading joints and links from URDF"); 62 | 63 | KDL::Tree tree; 64 | 65 | if (!kdl_parser::treeFromUrdfModel(robot_model, tree)) { 66 | RCLCPP_FATAL(LOGGER, "Failed to extract kdl tree from xml robot description"); 67 | } 68 | 69 | if (!tree.getChain(base_link, tip_link, chain)) { 70 | RCLCPP_FATAL_STREAM( 71 | LOGGER, "Couldn't find chain " << base_link.c_str() << " to " << tip_link.c_str()); 72 | } 73 | 74 | std::vector chain_segs = chain.segments; 75 | 76 | urdf::JointConstSharedPtr joint; 77 | 78 | std::vector l_bounds, u_bounds; 79 | 80 | lb.resize(chain.getNrOfJoints()); 81 | ub.resize(chain.getNrOfJoints()); 82 | 83 | uint joint_num = 0; 84 | for (unsigned int i = 0; i < chain_segs.size(); ++i) { 85 | joint = robot_model.getJoint(chain_segs[i].getJoint().getName()); 86 | if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) { 87 | joint_num++; 88 | float lower, upper; 89 | int hasLimits; 90 | if (joint->type != urdf::Joint::CONTINUOUS) { 91 | if (joint->safety) { 92 | lower = std::max(joint->limits->lower, joint->safety->soft_lower_limit); 93 | upper = std::min(joint->limits->upper, joint->safety->soft_upper_limit); 94 | } else { 95 | lower = joint->limits->lower; 96 | upper = joint->limits->upper; 97 | } 98 | hasLimits = 1; 99 | } else { 100 | hasLimits = 0; 101 | } 102 | if (hasLimits) { 103 | lb(joint_num - 1) = lower; 104 | ub(joint_num - 1) = upper; 105 | } else { 106 | lb(joint_num - 1) = std::numeric_limits::lowest(); 107 | ub(joint_num - 1) = std::numeric_limits::max(); 108 | } 109 | RCLCPP_DEBUG_STREAM( 110 | LOGGER, 111 | "IK Using joint " << joint->name << " " << lb(joint_num - 1) << " " << ub(joint_num - 1)); 112 | } 113 | } 114 | 115 | initialize(); 116 | } 117 | 118 | 119 | TRAC_IK::TRAC_IK( 120 | const KDL::Chain & _chain, const KDL::JntArray & _q_min, 121 | const KDL::JntArray & _q_max, double _maxtime, double _eps, SolveType _type) 122 | : initialized(false), 123 | chain(_chain), 124 | lb(_q_min), 125 | ub(_q_max), 126 | eps(_eps), 127 | maxtime(std::chrono::duration(_maxtime)), 128 | solvetype(_type) 129 | { 130 | initialize(); 131 | } 132 | 133 | void TRAC_IK::initialize() 134 | { 135 | assert(chain.getNrOfJoints() == lb.data.size()); 136 | assert(chain.getNrOfJoints() == ub.data.size()); 137 | 138 | jacsolver.reset(new KDL::ChainJntToJacSolver(chain)); 139 | nl_solver.reset(new NLOPT_IK::NLOPT_IK(chain, lb, ub, maxtime.count(), eps, NLOPT_IK::SumSq)); 140 | iksolver.reset(new KDL::ChainIkSolverPos_TL(chain, lb, ub, maxtime.count(), eps, true, true)); 141 | 142 | for (uint i = 0; i < chain.segments.size(); i++) { 143 | std::string type = chain.segments[i].getJoint().getTypeName(); 144 | if (type.find("Rot") != std::string::npos) { 145 | if (ub(types.size()) >= std::numeric_limits::max() && 146 | lb(types.size()) <= std::numeric_limits::lowest()) 147 | { 148 | types.push_back(KDL::BasicJointType::Continuous); 149 | } else { 150 | types.push_back(KDL::BasicJointType::RotJoint); 151 | } 152 | } else if (type.find("Trans") != std::string::npos) { 153 | types.push_back(KDL::BasicJointType::TransJoint); 154 | } 155 | } 156 | 157 | assert(types.size() == static_cast(lb.data.size())); // NOLINT 158 | 159 | initialized = true; 160 | } 161 | 162 | bool TRAC_IK::unique_solution(const KDL::JntArray & sol) 163 | { 164 | for (uint i = 0; i < solutions.size(); i++) { 165 | if (myEqual(sol, solutions[i])) { 166 | return false; 167 | } 168 | } 169 | return true; 170 | } 171 | 172 | inline void normalizeAngle(double & val, const double & min, const double & max) 173 | { 174 | if (val > max) { 175 | // Find actual angle offset 176 | double diffangle = fmod(val - max, 2 * M_PI); 177 | // Add that to upper bound and go back a full rotation 178 | val = max + diffangle - 2 * M_PI; 179 | } 180 | 181 | if (val < min) { 182 | // Find actual angle offset 183 | double diffangle = fmod(min - val, 2 * M_PI); 184 | // Add that to upper bound and go back a full rotation 185 | val = min - diffangle + 2 * M_PI; 186 | } 187 | } 188 | 189 | inline void normalizeAngle(double & val, const double & target) 190 | { 191 | double new_target = target + M_PI; 192 | if (val > new_target) { 193 | // Find actual angle offset 194 | double diffangle = fmod(val - new_target, 2 * M_PI); 195 | // Add that to upper bound and go back a full rotation 196 | val = new_target + diffangle - 2 * M_PI; 197 | } 198 | 199 | new_target = target - M_PI; 200 | if (val < new_target) { 201 | // Find actual angle offset 202 | double diffangle = fmod(new_target - val, 2 * M_PI); 203 | // Add that to upper bound and go back a full rotation 204 | val = new_target - diffangle + 2 * M_PI; 205 | } 206 | } 207 | 208 | 209 | template 210 | bool TRAC_IK::runSolver( 211 | T1 & solver, T2 & other_solver, 212 | const KDL::JntArray & q_init, 213 | const KDL::Frame & p_in) 214 | { 215 | KDL::JntArray q_out; 216 | 217 | std::chrono::duration fulltime(maxtime); 218 | KDL::JntArray seed = q_init; 219 | 220 | while (true) { 221 | std::chrono::duration timediff(std::chrono::system_clock::now() - start_time); 222 | 223 | if (timediff >= fulltime) { 224 | break; 225 | } 226 | 227 | solver.setMaxtime((fulltime - timediff).count()); 228 | 229 | int RC = solver.CartToJnt(seed, p_in, q_out, bounds); 230 | if (RC >= 0) { 231 | switch (solvetype) { 232 | case Manip1: 233 | case Manip2: 234 | normalize_limits(q_init, q_out); 235 | break; 236 | default: 237 | normalize_seed(q_init, q_out); 238 | break; 239 | } 240 | mtx_.lock(); 241 | if (unique_solution(q_out)) { 242 | solutions.push_back(q_out); 243 | uint curr_size = solutions.size(); 244 | errors.resize(curr_size); 245 | mtx_.unlock(); 246 | double err, penalty; 247 | switch (solvetype) { 248 | case Manip1: 249 | penalty = manipPenalty(q_out); 250 | err = penalty * TRAC_IK::ManipValue1(q_out); 251 | break; 252 | case Manip2: 253 | penalty = manipPenalty(q_out); 254 | err = penalty * TRAC_IK::ManipValue2(q_out); 255 | break; 256 | default: 257 | err = TRAC_IK::JointErr(q_init, q_out); 258 | break; 259 | } 260 | mtx_.lock(); 261 | errors[curr_size - 1] = std::make_pair(err, curr_size - 1); 262 | } 263 | mtx_.unlock(); 264 | } 265 | 266 | if (!solutions.empty() && solvetype == Speed) { 267 | break; 268 | } 269 | 270 | for (unsigned int j = 0; j < seed.data.size(); j++) { 271 | if (types[j] == KDL::BasicJointType::Continuous) { 272 | seed(j) = fRand(q_init(j) - 2 * M_PI, q_init(j) + 2 * M_PI); 273 | } else { 274 | seed(j) = fRand(lb(j), ub(j)); 275 | } 276 | } 277 | } 278 | other_solver.abort(); 279 | 280 | solver.setMaxtime(fulltime.count()); 281 | 282 | return true; 283 | } 284 | 285 | 286 | void TRAC_IK::normalize_seed(const KDL::JntArray & seed, KDL::JntArray & solution) 287 | { 288 | // Make sure rotational joint values are within 1 revolution of seed; then 289 | // ensure joint limits are met. 290 | 291 | for (uint i = 0; i < lb.data.size(); i++) { 292 | if (types[i] == KDL::BasicJointType::TransJoint) { 293 | continue; 294 | } 295 | 296 | double target = seed(i); 297 | double val = solution(i); 298 | 299 | normalizeAngle(val, target); 300 | 301 | if (types[i] == KDL::BasicJointType::Continuous) { 302 | solution(i) = val; 303 | continue; 304 | } 305 | 306 | normalizeAngle(val, lb(i), ub(i)); 307 | 308 | solution(i) = val; 309 | } 310 | } 311 | 312 | void TRAC_IK::normalize_limits(const KDL::JntArray & seed, KDL::JntArray & solution) 313 | { 314 | // Make sure rotational joint values are within 1 revolution of middle of 315 | // limits; then ensure joint limits are met. 316 | 317 | for (uint i = 0; i < lb.data.size(); i++) { 318 | if (types[i] == KDL::BasicJointType::TransJoint) { 319 | continue; 320 | } 321 | 322 | double target = seed(i); 323 | 324 | if (types[i] == KDL::BasicJointType::RotJoint && types[i] != KDL::BasicJointType::Continuous) { 325 | target = (ub(i) + lb(i)) / 2.0; 326 | } 327 | 328 | double val = solution(i); 329 | 330 | normalizeAngle(val, target); 331 | 332 | if (types[i] == KDL::BasicJointType::Continuous) { 333 | solution(i) = val; 334 | continue; 335 | } 336 | 337 | normalizeAngle(val, lb(i), ub(i)); 338 | 339 | solution(i) = val; 340 | } 341 | } 342 | 343 | 344 | double TRAC_IK::manipPenalty(const KDL::JntArray & arr) 345 | { 346 | double penalty = 1.0; 347 | for (uint i = 0; i < arr.data.size(); i++) { 348 | if (types[i] == KDL::BasicJointType::Continuous) { 349 | continue; 350 | } 351 | double range = ub(i) - lb(i); 352 | penalty *= ((arr(i) - lb(i)) * (ub(i) - arr(i)) / (range * range)); 353 | } 354 | return std::max(0.0, 1.0 - exp(-1 * penalty)); 355 | } 356 | 357 | 358 | double TRAC_IK::ManipValue1(const KDL::JntArray & arr) 359 | { 360 | KDL::Jacobian jac(arr.data.size()); 361 | 362 | jacsolver->JntToJac(arr, jac); 363 | 364 | Eigen::JacobiSVD svdsolver(jac.data); 365 | Eigen::MatrixXd singular_values = svdsolver.singularValues(); 366 | 367 | double error = 1.0; 368 | for (unsigned int i = 0; i < singular_values.rows(); ++i) { 369 | error *= singular_values(i, 0); 370 | } 371 | return error; 372 | } 373 | 374 | double TRAC_IK::ManipValue2(const KDL::JntArray & arr) 375 | { 376 | KDL::Jacobian jac(arr.data.size()); 377 | 378 | jacsolver->JntToJac(arr, jac); 379 | 380 | Eigen::JacobiSVD svdsolver(jac.data); 381 | Eigen::MatrixXd singular_values = svdsolver.singularValues(); 382 | 383 | return singular_values.minCoeff() / singular_values.maxCoeff(); 384 | } 385 | 386 | 387 | int TRAC_IK::CartToJnt( 388 | const KDL::JntArray & q_init, const KDL::Frame & p_in, KDL::JntArray & q_out, 389 | const KDL::Twist & _bounds) 390 | { 391 | if (!initialized) { 392 | RCLCPP_ERROR( 393 | LOGGER, 394 | "TRAC-IK was not properly initialized with a valid chain or limits. IK cannot proceed"); 395 | return -1; 396 | } 397 | 398 | 399 | start_time = std::chrono::time_point>( 400 | std::chrono::system_clock::now()); 401 | 402 | nl_solver->reset(); 403 | iksolver->reset(); 404 | 405 | solutions.clear(); 406 | errors.clear(); 407 | 408 | bounds = _bounds; 409 | 410 | task1 = std::thread(&TRAC_IK::runKDL, this, q_init, p_in); 411 | task2 = std::thread(&TRAC_IK::runNLOPT, this, q_init, p_in); 412 | 413 | task1.join(); 414 | task2.join(); 415 | 416 | if (solutions.empty()) { 417 | q_out = q_init; 418 | return -3; 419 | } 420 | 421 | switch (solvetype) { 422 | case Manip1: 423 | case Manip2: 424 | std::sort(errors.rbegin(), errors.rend()); // rbegin/rend to sort by max 425 | break; 426 | default: 427 | std::sort(errors.begin(), errors.end()); 428 | break; 429 | } 430 | 431 | q_out = solutions[errors[0].second]; 432 | 433 | return solutions.size(); 434 | } 435 | 436 | 437 | TRAC_IK::~TRAC_IK() 438 | { 439 | if (task1.joinable()) { 440 | task1.join(); 441 | } 442 | if (task2.joinable()) { 443 | task2.join(); 444 | } 445 | } 446 | } // namespace TRAC_IK 447 | -------------------------------------------------------------------------------- /trac_ik_kinematics_plugin/src/trac_ik_kinematics_plugin.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************** 2 | Copyright (c) 2015, TRACLabs, Inc. 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without modification, 6 | are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, 9 | this list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software 17 | without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 22 | IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 23 | INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 24 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 25 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 26 | LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 28 | OF THE POSSIBILITY OF SUCH DAMAGE. 29 | ********************************************************************************/ 30 | 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | namespace trac_ik_kinematics_plugin 43 | { 44 | 45 | bool TRAC_IKKinematicsPlugin::initialize(const std::string &robot_description, 46 | const std::string& group_name, 47 | const std::string& base_name, 48 | const std::string& tip_name, 49 | double search_discretization) 50 | { 51 | std::vector tip_names = {tip_name}; 52 | setValues(robot_description, group_name, base_name, tip_names, search_discretization); 53 | 54 | ros::NodeHandle node_handle("~"); 55 | 56 | urdf::Model robot_model; 57 | std::string xml_string; 58 | 59 | std::string urdf_xml, full_urdf_xml; 60 | node_handle.param("urdf_xml", urdf_xml, robot_description); 61 | node_handle.searchParam(urdf_xml, full_urdf_xml); 62 | 63 | ROS_DEBUG_NAMED("trac_ik", "Reading xml file from parameter server"); 64 | if (!node_handle.getParam(full_urdf_xml, xml_string)) 65 | { 66 | ROS_FATAL_NAMED("trac_ik", "Could not load the xml from parameter server: %s", urdf_xml.c_str()); 67 | return false; 68 | } 69 | 70 | node_handle.param(full_urdf_xml, xml_string, std::string()); 71 | robot_model.initString(xml_string); 72 | 73 | ROS_DEBUG_STREAM_NAMED("trac_ik", "Reading joints and links from URDF"); 74 | 75 | KDL::Tree tree; 76 | 77 | if (!kdl_parser::treeFromUrdfModel(robot_model, tree)) 78 | { 79 | ROS_FATAL("Failed to extract kdl tree from xml robot description"); 80 | return false; 81 | } 82 | 83 | if (!tree.getChain(base_name, tip_name, chain)) 84 | { 85 | ROS_FATAL("Couldn't find chain %s to %s", base_name.c_str(), tip_name.c_str()); 86 | return false; 87 | } 88 | 89 | num_joints_ = chain.getNrOfJoints(); 90 | 91 | std::vector chain_segs = chain.segments; 92 | 93 | urdf::JointConstSharedPtr joint; 94 | 95 | std::vector l_bounds, u_bounds; 96 | 97 | joint_min.resize(num_joints_); 98 | joint_max.resize(num_joints_); 99 | 100 | uint joint_num = 0; 101 | for (unsigned int i = 0; i < chain_segs.size(); ++i) 102 | { 103 | 104 | link_names_.push_back(chain_segs[i].getName()); 105 | joint = robot_model.getJoint(chain_segs[i].getJoint().getName()); 106 | if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) 107 | { 108 | joint_num++; 109 | assert(joint_num <= num_joints_); 110 | float lower, upper; 111 | int hasLimits; 112 | joint_names_.push_back(joint->name); 113 | if (joint->type != urdf::Joint::CONTINUOUS) 114 | { 115 | if (joint->safety) 116 | { 117 | lower = std::max(joint->limits->lower, joint->safety->soft_lower_limit); 118 | upper = std::min(joint->limits->upper, joint->safety->soft_upper_limit); 119 | } 120 | else 121 | { 122 | lower = joint->limits->lower; 123 | upper = joint->limits->upper; 124 | } 125 | hasLimits = 1; 126 | } 127 | else 128 | { 129 | hasLimits = 0; 130 | } 131 | if (hasLimits) 132 | { 133 | joint_min(joint_num - 1) = lower; 134 | joint_max(joint_num - 1) = upper; 135 | } 136 | else 137 | { 138 | joint_min(joint_num - 1) = std::numeric_limits::lowest(); 139 | joint_max(joint_num - 1) = std::numeric_limits::max(); 140 | } 141 | ROS_INFO_STREAM("IK Using joint " << chain_segs[i].getName() << " " << joint_min(joint_num - 1) << " " << joint_max(joint_num - 1)); 142 | } 143 | } 144 | 145 | ROS_INFO_NAMED("trac-ik plugin", "Looking in common namespaces for param name: %s", (group_name + "/position_only_ik").c_str()); 146 | lookupParam(group_name + "/position_only_ik", position_ik_, false); 147 | ROS_INFO_NAMED("trac-ik plugin", "Looking in common namespaces for param name: %s", (group_name + "/solve_type").c_str()); 148 | lookupParam(group_name + "/solve_type", solve_type, std::string("Speed")); 149 | ROS_INFO_NAMED("trac_ik plugin", "Using solve type %s", solve_type.c_str()); 150 | 151 | active_ = true; 152 | return true; 153 | } 154 | 155 | 156 | int TRAC_IKKinematicsPlugin::getKDLSegmentIndex(const std::string &name) const 157 | { 158 | int i = 0; 159 | while (i < (int)chain.getNrOfSegments()) 160 | { 161 | if (chain.getSegment(i).getName() == name) 162 | { 163 | return i + 1; 164 | } 165 | i++; 166 | } 167 | return -1; 168 | } 169 | 170 | 171 | bool TRAC_IKKinematicsPlugin::getPositionFK(const std::vector &link_names, 172 | const std::vector &joint_angles, 173 | std::vector &poses) const 174 | { 175 | if (!active_) 176 | { 177 | ROS_ERROR_NAMED("trac_ik", "kinematics not active"); 178 | return false; 179 | } 180 | poses.resize(link_names.size()); 181 | if (joint_angles.size() != num_joints_) 182 | { 183 | ROS_ERROR_NAMED("trac_ik", "Joint angles vector must have size: %d", num_joints_); 184 | return false; 185 | } 186 | 187 | KDL::Frame p_out; 188 | geometry_msgs::PoseStamped pose; 189 | tf::Stamped tf_pose; 190 | 191 | KDL::JntArray jnt_pos_in(num_joints_); 192 | for (unsigned int i = 0; i < num_joints_; i++) 193 | { 194 | jnt_pos_in(i) = joint_angles[i]; 195 | } 196 | 197 | KDL::ChainFkSolverPos_recursive fk_solver(chain); 198 | 199 | bool valid = true; 200 | for (unsigned int i = 0; i < poses.size(); i++) 201 | { 202 | ROS_DEBUG_NAMED("trac_ik", "End effector index: %d", getKDLSegmentIndex(link_names[i])); 203 | if (fk_solver.JntToCart(jnt_pos_in, p_out, getKDLSegmentIndex(link_names[i])) >= 0) 204 | { 205 | tf::poseKDLToMsg(p_out, poses[i]); 206 | } 207 | else 208 | { 209 | ROS_ERROR_NAMED("trac_ik", "Could not compute FK for %s", link_names[i].c_str()); 210 | valid = false; 211 | } 212 | } 213 | 214 | return valid; 215 | } 216 | 217 | 218 | bool TRAC_IKKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose, 219 | const std::vector &ik_seed_state, 220 | std::vector &solution, 221 | moveit_msgs::MoveItErrorCodes &error_code, 222 | const kinematics::KinematicsQueryOptions &options) const 223 | { 224 | const IKCallbackFn solution_callback = 0; 225 | std::vector consistency_limits; 226 | 227 | return searchPositionIK(ik_pose, 228 | ik_seed_state, 229 | default_timeout_, 230 | solution, 231 | solution_callback, 232 | error_code, 233 | consistency_limits, 234 | options); 235 | } 236 | 237 | bool TRAC_IKKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, 238 | const std::vector &ik_seed_state, 239 | double timeout, 240 | std::vector &solution, 241 | moveit_msgs::MoveItErrorCodes &error_code, 242 | const kinematics::KinematicsQueryOptions &options) const 243 | { 244 | const IKCallbackFn solution_callback = 0; 245 | std::vector consistency_limits; 246 | 247 | return searchPositionIK(ik_pose, 248 | ik_seed_state, 249 | timeout, 250 | solution, 251 | solution_callback, 252 | error_code, 253 | consistency_limits, 254 | options); 255 | } 256 | 257 | bool TRAC_IKKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, 258 | const std::vector &ik_seed_state, 259 | double timeout, 260 | const std::vector &consistency_limits, 261 | std::vector &solution, 262 | moveit_msgs::MoveItErrorCodes &error_code, 263 | const kinematics::KinematicsQueryOptions &options) const 264 | { 265 | const IKCallbackFn solution_callback = 0; 266 | return searchPositionIK(ik_pose, 267 | ik_seed_state, 268 | timeout, 269 | solution, 270 | solution_callback, 271 | error_code, 272 | consistency_limits, 273 | options); 274 | } 275 | 276 | bool TRAC_IKKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, 277 | const std::vector &ik_seed_state, 278 | double timeout, 279 | std::vector &solution, 280 | const IKCallbackFn &solution_callback, 281 | moveit_msgs::MoveItErrorCodes &error_code, 282 | const kinematics::KinematicsQueryOptions &options) const 283 | { 284 | std::vector consistency_limits; 285 | return searchPositionIK(ik_pose, 286 | ik_seed_state, 287 | timeout, 288 | solution, 289 | solution_callback, 290 | error_code, 291 | consistency_limits, 292 | options); 293 | } 294 | 295 | bool TRAC_IKKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, 296 | const std::vector &ik_seed_state, 297 | double timeout, 298 | const std::vector &consistency_limits, 299 | std::vector &solution, 300 | const IKCallbackFn &solution_callback, 301 | moveit_msgs::MoveItErrorCodes &error_code, 302 | const kinematics::KinematicsQueryOptions &options) const 303 | { 304 | return searchPositionIK(ik_pose, 305 | ik_seed_state, 306 | timeout, 307 | solution, 308 | solution_callback, 309 | error_code, 310 | consistency_limits, 311 | options); 312 | } 313 | 314 | bool TRAC_IKKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, 315 | const std::vector &ik_seed_state, 316 | double timeout, 317 | std::vector &solution, 318 | const IKCallbackFn &solution_callback, 319 | moveit_msgs::MoveItErrorCodes &error_code, 320 | const std::vector &consistency_limits, 321 | const kinematics::KinematicsQueryOptions &options) const 322 | { 323 | ROS_DEBUG_STREAM_NAMED("trac_ik", "getPositionIK"); 324 | 325 | if (!active_) 326 | { 327 | ROS_ERROR("kinematics not active"); 328 | error_code.val = error_code.NO_IK_SOLUTION; 329 | return false; 330 | } 331 | 332 | if (ik_seed_state.size() != num_joints_) 333 | { 334 | ROS_ERROR_STREAM_NAMED("trac_ik", "Seed state must have size " << num_joints_ << " instead of size " << ik_seed_state.size()); 335 | error_code.val = error_code.NO_IK_SOLUTION; 336 | return false; 337 | } 338 | 339 | KDL::Frame frame; 340 | tf::poseMsgToKDL(ik_pose, frame); 341 | 342 | KDL::JntArray in(num_joints_), out(num_joints_); 343 | 344 | for (uint z = 0; z < num_joints_; z++) 345 | in(z) = ik_seed_state[z]; 346 | 347 | KDL::Twist bounds = KDL::Twist::Zero(); 348 | 349 | if (position_ik_) 350 | { 351 | bounds.rot.x(std::numeric_limits::max()); 352 | bounds.rot.y(std::numeric_limits::max()); 353 | bounds.rot.z(std::numeric_limits::max()); 354 | } 355 | 356 | double epsilon = 1e-5; //Same as MoveIt's KDL plugin 357 | 358 | TRAC_IK::SolveType solvetype; 359 | 360 | if (solve_type == "Manipulation1") 361 | solvetype = TRAC_IK::Manip1; 362 | else if (solve_type == "Manipulation2") 363 | solvetype = TRAC_IK::Manip2; 364 | else if (solve_type == "Distance") 365 | solvetype = TRAC_IK::Distance; 366 | else 367 | { 368 | if (solve_type != "Speed") 369 | { 370 | ROS_WARN_STREAM_NAMED("trac_ik", solve_type << " is not a valid solve_type; setting to default: Speed"); 371 | } 372 | solvetype = TRAC_IK::Speed; 373 | } 374 | 375 | TRAC_IK::TRAC_IK ik_solver(chain, joint_min, joint_max, timeout, epsilon, solvetype); 376 | 377 | int rc = ik_solver.CartToJnt(in, frame, out, bounds); 378 | 379 | 380 | solution.resize(num_joints_); 381 | 382 | if (rc >= 0) 383 | { 384 | for (uint z = 0; z < num_joints_; z++) 385 | solution[z] = out(z); 386 | 387 | // check for collisions if a callback is provided 388 | if (!solution_callback.empty()) 389 | { 390 | solution_callback(ik_pose, solution, error_code); 391 | if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) 392 | { 393 | ROS_DEBUG_STREAM_NAMED("trac_ik", "Solution passes callback"); 394 | return true; 395 | } 396 | else 397 | { 398 | ROS_DEBUG_STREAM_NAMED("trac_ik", "Solution has error code " << error_code); 399 | return false; 400 | } 401 | } 402 | else 403 | return true; // no collision check callback provided 404 | } 405 | 406 | error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; 407 | return false; 408 | } 409 | 410 | 411 | 412 | } // end namespace 413 | 414 | //register TRAC_IKKinematicsPlugin as a KinematicsBase implementation 415 | #include 416 | PLUGINLIB_EXPORT_CLASS(trac_ik_kinematics_plugin::TRAC_IKKinematicsPlugin, kinematics::KinematicsBase); 417 | -------------------------------------------------------------------------------- /trac_ik_lib/src/nlopt_ik.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, TRACLabs, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | #include "trac_ik/nlopt_ik.hpp" 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include "rclcpp/rclcpp.hpp" 40 | #include "trac_ik/dual_quaternion.h" 41 | 42 | 43 | namespace NLOPT_IK 44 | { 45 | static const rclcpp::Logger LOGGER = rclcpp::get_logger("trac_ik.ros.nlopt_ik_solver"); 46 | 47 | dual_quaternion targetDQ; 48 | 49 | double minfunc(const std::vector & x, std::vector & grad, void * data) 50 | { 51 | // Auxilory function to minimize (Sum of Squared joint angle error 52 | // from the requested configuration). Because we wanted a Class 53 | // without static members, but NLOpt library does not support 54 | // passing methods of Classes, we use these auxilary functions. 55 | 56 | NLOPT_IK * c = reinterpret_cast(data); 57 | 58 | return c->minJoints(x, grad); 59 | } 60 | 61 | double minfuncDQ(const std::vector & x, std::vector & grad, void * data) 62 | { 63 | // Auxilory function to minimize (Sum of Squared joint angle error 64 | // from the requested configuration). Because we wanted a Class 65 | // without static members, but NLOpt library does not support 66 | // passing methods of Classes, we use these auxilary functions. 67 | NLOPT_IK * c = reinterpret_cast(data); 68 | 69 | std::vector vals(x); 70 | 71 | double jump = std::numeric_limits::epsilon(); 72 | double result[1]; 73 | c->cartDQError(vals, result); 74 | 75 | if (!grad.empty()) { 76 | double v1[1]; 77 | for (uint i = 0; i < x.size(); i++) { 78 | double original = vals[i]; 79 | 80 | vals[i] = original + jump; 81 | c->cartDQError(vals, v1); 82 | 83 | vals[i] = original; 84 | grad[i] = (v1[0] - result[0]) / (2 * jump); 85 | } 86 | } 87 | 88 | return result[0]; 89 | } 90 | 91 | 92 | double minfuncSumSquared(const std::vector & x, std::vector & grad, void * data) 93 | { 94 | // Auxilory function to minimize (Sum of Squared joint angle error 95 | // from the requested configuration). Because we wanted a Class 96 | // without static members, but NLOpt library does not support 97 | // passing methods of Classes, we use these auxilary functions. 98 | 99 | NLOPT_IK * c = reinterpret_cast(data); 100 | 101 | std::vector vals(x); 102 | 103 | double jump = std::numeric_limits::epsilon(); 104 | double result[1]; 105 | c->cartSumSquaredError(vals, result); 106 | 107 | if (!grad.empty()) { 108 | double v1[1]; 109 | for (uint i = 0; i < x.size(); i++) { 110 | double original = vals[i]; 111 | 112 | vals[i] = original + jump; 113 | c->cartSumSquaredError(vals, v1); 114 | 115 | vals[i] = original; 116 | grad[i] = (v1[0] - result[0]) / (2.0 * jump); 117 | } 118 | } 119 | 120 | return result[0]; 121 | } 122 | 123 | 124 | double minfuncL2(const std::vector & x, std::vector & grad, void * data) 125 | { 126 | // Auxilory function to minimize (Sum of Squared joint angle error 127 | // from the requested configuration). Because we wanted a Class 128 | // without static members, but NLOpt library does not support 129 | // passing methods of Classes, we use these auxilary functions. 130 | 131 | NLOPT_IK * c = reinterpret_cast(data); 132 | 133 | std::vector vals(x); 134 | 135 | double jump = std::numeric_limits::epsilon(); 136 | double result[1]; 137 | c->cartL2NormError(vals, result); 138 | 139 | if (!grad.empty()) { 140 | double v1[1]; 141 | for (uint i = 0; i < x.size(); i++) { 142 | double original = vals[i]; 143 | 144 | vals[i] = original + jump; 145 | c->cartL2NormError(vals, v1); 146 | 147 | vals[i] = original; 148 | grad[i] = (v1[0] - result[0]) / (2.0 * jump); 149 | } 150 | } 151 | 152 | return result[0]; 153 | } 154 | 155 | 156 | void constrainfuncm(uint m, double * result, uint n, const double * x, double * grad, void * data) 157 | { 158 | // Equality constraint auxilary function for Euclidean distance . 159 | // This also uses a small walk to approximate the gradient of the 160 | // constraint function at the current joint angles. 161 | 162 | NLOPT_IK * c = reinterpret_cast(data); 163 | 164 | std::vector vals(n); 165 | 166 | for (uint i = 0; i < n; i++) { 167 | vals[i] = x[i]; 168 | } 169 | 170 | double jump = std::numeric_limits::epsilon(); 171 | 172 | c->cartSumSquaredError(vals, result); 173 | 174 | if (grad != NULL) { 175 | std::vector v1(m); 176 | for (uint i = 0; i < n; i++) { 177 | double o = vals[i]; 178 | vals[i] = o + jump; 179 | c->cartSumSquaredError(vals, v1.data()); 180 | vals[i] = o; 181 | for (uint j = 0; j < m; j++) { 182 | grad[j * n + i] = (v1[j] - result[j]) / (2 * jump); 183 | } 184 | } 185 | } 186 | } 187 | 188 | 189 | NLOPT_IK::NLOPT_IK( 190 | const KDL::Chain & _chain, const KDL::JntArray & _q_min, 191 | const KDL::JntArray & _q_max, double _maxtime, double _eps, OptType _type) 192 | : chain(_chain), fksolver(chain), maxtime(std::chrono::duration(_maxtime)), 193 | eps(std::abs(_eps)), TYPE(_type) 194 | { 195 | assert(chain.getNrOfJoints() == _q_min.data.size()); 196 | assert(chain.getNrOfJoints() == _q_max.data.size()); 197 | 198 | // Constructor for an IK Class. Takes in a Chain to operate on, 199 | // the min and max joint limits, an (optional) maximum number of 200 | // iterations, and an (optional) desired error. 201 | reset(); 202 | 203 | if (chain.getNrOfJoints() < 2) { 204 | rclcpp::Clock steady_clock(RCL_STEADY_TIME); 205 | RCLCPP_WARN_THROTTLE( 206 | LOGGER, steady_clock, 1.0, 207 | "NLOpt_IK can only be run for chains of length 2 or more"); 208 | return; 209 | } 210 | opt = nlopt::opt(nlopt::LD_SLSQP, _chain.getNrOfJoints()); 211 | 212 | for (uint i = 0; i < chain.getNrOfJoints(); i++) { 213 | lb.push_back(_q_min(i)); 214 | ub.push_back(_q_max(i)); 215 | } 216 | 217 | for (uint i = 0; i < chain.segments.size(); i++) { 218 | std::string type = chain.segments[i].getJoint().getTypeName(); 219 | if (type.find("Rot") != std::string::npos) { 220 | if (_q_max(types.size()) >= std::numeric_limits::max() && 221 | _q_min(types.size()) <= std::numeric_limits::lowest()) 222 | { 223 | types.push_back(KDL::BasicJointType::Continuous); 224 | } else { 225 | types.push_back(KDL::BasicJointType::RotJoint); 226 | } 227 | } else if (type.find("Trans") != std::string::npos) { 228 | types.push_back(KDL::BasicJointType::TransJoint); 229 | } 230 | } 231 | 232 | assert(types.size() == lb.size()); 233 | 234 | std::vector tolerance(1, std::numeric_limits::epsilon()); 235 | opt.set_xtol_abs(tolerance[0]); 236 | 237 | 238 | switch (TYPE) { 239 | case Joint: 240 | opt.set_min_objective(minfunc, this); 241 | opt.add_equality_mconstraint(constrainfuncm, this, tolerance); 242 | break; 243 | case DualQuat: 244 | opt.set_min_objective(minfuncDQ, this); 245 | break; 246 | case SumSq: 247 | opt.set_min_objective(minfuncSumSquared, this); 248 | break; 249 | case L2: 250 | opt.set_min_objective(minfuncL2, this); 251 | break; 252 | } 253 | } 254 | 255 | 256 | double NLOPT_IK::minJoints(const std::vector & x, std::vector & grad) 257 | { 258 | // Actual function to compute the error between the current joint 259 | // configuration and the desired. The SSE is easy to provide a 260 | // closed form gradient for. 261 | 262 | bool gradient = !grad.empty(); 263 | 264 | double err = 0; 265 | for (uint i = 0; i < x.size(); i++) { 266 | err += pow(x[i] - des[i], 2); 267 | if (gradient) { 268 | grad[i] = 2.0 * (x[i] - des[i]); 269 | } 270 | } 271 | 272 | return err; 273 | } 274 | 275 | 276 | void NLOPT_IK::cartSumSquaredError(const std::vector & x, double error[]) 277 | { 278 | // Actual function to compute Euclidean distance error. This uses 279 | // the KDL Forward Kinematics solver to compute the Cartesian pose 280 | // of the current joint configuration and compares that to the 281 | // desired Cartesian pose for the IK solve. 282 | 283 | if (aborted || progress != -3) { 284 | opt.force_stop(); 285 | return; 286 | } 287 | 288 | 289 | KDL::JntArray q(x.size()); 290 | 291 | for (uint i = 0; i < x.size(); i++) { 292 | q(i) = x[i]; 293 | } 294 | 295 | int rc = fksolver.JntToCart(q, currentPose); 296 | 297 | if (rc < 0) { 298 | RCLCPP_FATAL_STREAM(LOGGER, "KDL FKSolver is failing: " << q.data); 299 | } 300 | 301 | if (std::isnan(currentPose.p.x())) { 302 | RCLCPP_ERROR(LOGGER, "NaNs from NLOpt!!"); 303 | error[0] = std::numeric_limits::max(); 304 | progress = -1; 305 | return; 306 | } 307 | 308 | KDL::Twist delta_twist = KDL::diffRelative(targetPose, currentPose); 309 | 310 | for (int i = 0; i < 6; i++) { 311 | if (std::abs(delta_twist[i]) <= std::abs(bounds[i])) { 312 | delta_twist[i] = 0.0; 313 | } 314 | } 315 | 316 | error[0] = 317 | KDL::dot(delta_twist.vel, delta_twist.vel) + KDL::dot(delta_twist.rot, delta_twist.rot); 318 | 319 | if (KDL::Equal(delta_twist, KDL::Twist::Zero(), eps)) { 320 | progress = 1; 321 | best_x = x; 322 | return; 323 | } 324 | } 325 | 326 | 327 | void NLOPT_IK::cartL2NormError(const std::vector & x, double error[]) 328 | { 329 | // Actual function to compute Euclidean distance error. This uses 330 | // the KDL Forward Kinematics solver to compute the Cartesian pose 331 | // of the current joint configuration and compares that to the 332 | // desired Cartesian pose for the IK solve. 333 | 334 | if (aborted || progress != -3) { 335 | opt.force_stop(); 336 | return; 337 | } 338 | 339 | KDL::JntArray q(x.size()); 340 | 341 | for (uint i = 0; i < x.size(); i++) { 342 | q(i) = x[i]; 343 | } 344 | 345 | int rc = fksolver.JntToCart(q, currentPose); 346 | 347 | if (rc < 0) { 348 | RCLCPP_FATAL_STREAM(LOGGER, "KDL FKSolver is failing: " << q.data); 349 | } 350 | 351 | 352 | if (std::isnan(currentPose.p.x())) { 353 | RCLCPP_ERROR(LOGGER, "NaNs from NLOpt!!"); 354 | error[0] = std::numeric_limits::max(); 355 | progress = -1; 356 | return; 357 | } 358 | 359 | KDL::Twist delta_twist = KDL::diffRelative(targetPose, currentPose); 360 | 361 | for (int i = 0; i < 6; i++) { 362 | if (std::abs(delta_twist[i]) <= std::abs(bounds[i])) { 363 | delta_twist[i] = 0.0; 364 | } 365 | } 366 | 367 | error[0] = 368 | std::sqrt( 369 | KDL::dot( 370 | delta_twist.vel, 371 | delta_twist.vel) + KDL::dot(delta_twist.rot, delta_twist.rot)); 372 | 373 | if (KDL::Equal(delta_twist, KDL::Twist::Zero(), eps)) { 374 | progress = 1; 375 | best_x = x; 376 | return; 377 | } 378 | } 379 | 380 | 381 | void NLOPT_IK::cartDQError(const std::vector & x, double error[]) 382 | { 383 | // Actual function to compute Euclidean distance error. This uses 384 | // the KDL Forward Kinematics solver to compute the Cartesian pose 385 | // of the current joint configuration and compares that to the 386 | // desired Cartesian pose for the IK solve. 387 | 388 | if (aborted || progress != -3) { 389 | opt.force_stop(); 390 | return; 391 | } 392 | 393 | KDL::JntArray q(x.size()); 394 | 395 | for (uint i = 0; i < x.size(); i++) { 396 | q(i) = x[i]; 397 | } 398 | 399 | int rc = fksolver.JntToCart(q, currentPose); 400 | 401 | if (rc < 0) { 402 | RCLCPP_FATAL_STREAM(LOGGER, "KDL FKSolver is failing: " << q.data); 403 | } 404 | 405 | 406 | if (std::isnan(currentPose.p.x())) { 407 | RCLCPP_ERROR(LOGGER, "NaNs from NLOpt!!"); 408 | error[0] = std::numeric_limits::max(); 409 | progress = -1; 410 | return; 411 | } 412 | 413 | KDL::Twist delta_twist = KDL::diffRelative(targetPose, currentPose); 414 | 415 | for (int i = 0; i < 6; i++) { 416 | if (std::abs(delta_twist[i]) <= std::abs(bounds[i])) { 417 | delta_twist[i] = 0.0; 418 | } 419 | } 420 | 421 | math3d::matrix3x3 currentRotationMatrix(currentPose.M.data); 422 | math3d::quaternion currentQuaternion = math3d::rot_matrix_to_quaternion( 423 | currentRotationMatrix); 424 | math3d::point3d currentTranslation(currentPose.p.data); 425 | dual_quaternion currentDQ = dual_quaternion::rigid_transformation( 426 | currentQuaternion, 427 | currentTranslation); 428 | 429 | dual_quaternion errorDQ = (currentDQ * !targetDQ).normalize(); 430 | errorDQ.log(); 431 | error[0] = 4.0f * dot(errorDQ, errorDQ); 432 | 433 | 434 | if (KDL::Equal(delta_twist, KDL::Twist::Zero(), eps)) { 435 | progress = 1; 436 | best_x = x; 437 | return; 438 | } 439 | } 440 | 441 | 442 | int NLOPT_IK::CartToJnt( 443 | const KDL::JntArray & q_init, const KDL::Frame & p_in, 444 | KDL::JntArray & q_out, const KDL::Twist _bounds, 445 | const KDL::JntArray & q_desired) 446 | { 447 | // User command to start an IK solve. Takes in a seed 448 | // configuration, a Cartesian pose, and (optional) a desired 449 | // configuration. If the desired is not provided, the seed is 450 | // used. Outputs the joint configuration found that solves the 451 | // IK. 452 | 453 | // Returns -3 if a configuration could not be found within the eps 454 | // set up in the constructor. 455 | 456 | std::chrono::time_point> start_time( 457 | std::chrono::system_clock::now()); 458 | 459 | bounds = _bounds; 460 | q_out = q_init; 461 | 462 | if (chain.getNrOfJoints() < 2) { 463 | rclcpp::Clock steady_clock(RCL_STEADY_TIME); 464 | RCLCPP_ERROR_THROTTLE( 465 | LOGGER, steady_clock, 1.0, 466 | "NLOpt_IK can only be run for chains of length 2 or more"); 467 | return -3; 468 | } 469 | 470 | if (static_cast(q_init.data.size()) != types.size()) { // NOLINT 471 | rclcpp::Clock steady_clock(RCL_STEADY_TIME); 472 | RCLCPP_ERROR_THROTTLE( 473 | LOGGER, steady_clock, 1.0, 474 | "IK seeded with wrong number of joints. Expected %d but got %d", 475 | (int)types.size(), (int)q_init.data.size()); 476 | return -3; 477 | } 478 | 479 | opt.set_maxtime(maxtime.count()); 480 | 481 | 482 | double minf; /* the minimum objective value, upon return */ 483 | 484 | targetPose = p_in; 485 | 486 | if (TYPE == 1) { // DQ 487 | math3d::matrix3x3 targetRotationMatrix(targetPose.M.data); 488 | math3d::quaternion targetQuaternion = math3d::rot_matrix_to_quaternion( 489 | targetRotationMatrix); 490 | math3d::point3d targetTranslation(targetPose.p.data); 491 | targetDQ = dual_quaternion::rigid_transformation(targetQuaternion, targetTranslation); 492 | } 493 | // else if (TYPE == 1) 494 | // { 495 | // z_target = targetPose*z_up; 496 | // x_target = targetPose*x_out; 497 | // y_target = targetPose*y_out; 498 | // } 499 | 500 | 501 | // fksolver.JntToCart(q_init,currentPose); 502 | 503 | std::vector x(chain.getNrOfJoints()); 504 | 505 | for (uint i = 0; i < x.size(); i++) { 506 | x[i] = q_init(i); 507 | 508 | if (types[i] == KDL::BasicJointType::Continuous) { 509 | continue; 510 | } 511 | 512 | if (types[i] == KDL::BasicJointType::TransJoint) { 513 | x[i] = std::min(x[i], ub[i]); 514 | x[i] = std::max(x[i], lb[i]); 515 | } else { 516 | // Below is to handle bad seeds outside of limits 517 | 518 | if (x[i] > ub[i]) { 519 | // Find actual angle offset 520 | double diffangle = fmod(x[i] - ub[i], 2 * M_PI); 521 | // Add that to upper bound and go back a full rotation 522 | x[i] = ub[i] + diffangle - 2 * M_PI; 523 | } 524 | 525 | if (x[i] < lb[i]) { 526 | // Find actual angle offset 527 | double diffangle = fmod(lb[i] - x[i], 2 * M_PI); 528 | // Subtract that from lower bound and go forward a full rotation 529 | x[i] = lb[i] - diffangle + 2 * M_PI; 530 | } 531 | 532 | if (x[i] > ub[i]) { 533 | x[i] = (ub[i] + lb[i]) / 2.0; 534 | } 535 | } 536 | } 537 | 538 | best_x = x; 539 | progress = -3; 540 | 541 | std::vector artificial_lower_limits(lb.size()); 542 | 543 | for (uint i = 0; i < lb.size(); i++) { 544 | if (types[i] == KDL::BasicJointType::Continuous) { 545 | artificial_lower_limits[i] = best_x[i] - 2 * M_PI; 546 | } else if (types[i] == KDL::BasicJointType::TransJoint) { 547 | artificial_lower_limits[i] = lb[i]; 548 | } else { 549 | artificial_lower_limits[i] = std::max(lb[i], best_x[i] - 2 * M_PI); 550 | } 551 | } 552 | 553 | opt.set_lower_bounds(artificial_lower_limits); 554 | 555 | std::vector artificial_upper_limits(lb.size()); 556 | 557 | for (uint i = 0; i < ub.size(); i++) { 558 | if (types[i] == KDL::BasicJointType::Continuous) { 559 | artificial_upper_limits[i] = best_x[i] + 2 * M_PI; 560 | } else if (types[i] == KDL::BasicJointType::TransJoint) { 561 | artificial_upper_limits[i] = ub[i]; 562 | } else { 563 | artificial_upper_limits[i] = std::min(ub[i], best_x[i] + 2 * M_PI); 564 | } 565 | } 566 | 567 | opt.set_upper_bounds(artificial_upper_limits); 568 | 569 | if (q_desired.data.size() == 0) { 570 | des = x; 571 | } else { 572 | des.resize(x.size()); 573 | for (uint i = 0; i < des.size(); i++) { 574 | des[i] = q_desired(i); 575 | } 576 | } 577 | 578 | try { 579 | opt.optimize(x, minf); 580 | } catch (...) { 581 | } 582 | 583 | if (progress == -1) { // Got NaNs 584 | progress = -3; 585 | } 586 | 587 | 588 | if (!aborted && progress < 0) { 589 | std::chrono::duration diff(std::chrono::system_clock::now() - start_time); 590 | 591 | while (diff < maxtime && !aborted && progress < 0) { 592 | for (uint i = 0; i < x.size(); i++) { 593 | x[i] = fRand(artificial_lower_limits[i], artificial_upper_limits[i]); 594 | } 595 | 596 | opt.set_maxtime((maxtime - diff).count()); 597 | 598 | try { 599 | opt.optimize(x, minf); 600 | } catch (...) { 601 | } 602 | 603 | if (progress == -1) { // Got NaNs 604 | progress = -3; 605 | } 606 | 607 | diff = std::chrono::system_clock::now() - start_time; 608 | } 609 | } 610 | 611 | 612 | for (uint i = 0; i < x.size(); i++) { 613 | q_out(i) = best_x[i]; 614 | } 615 | 616 | return progress; 617 | } 618 | 619 | 620 | } // namespace NLOPT_IK 621 | -------------------------------------------------------------------------------- /trac_ik_lib/include/trac_ik/math3d.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, TRACLabs, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef TRAC_IK__MATH3D_H_ 30 | #define TRAC_IK__MATH3D_H_ 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #ifndef M_PI 41 | #define M_PI (3.14159265358979323846) 42 | #endif 43 | 44 | typedef unsigned char uint8_t; 45 | typedef unsigned int uint32_t; 46 | 47 | namespace math3d 48 | { 49 | 50 | static const double pi = M_PI; 51 | static const double rad_on_deg = pi / 180.; 52 | static const double deg_on_rad = 180. / pi; 53 | 54 | template 55 | inline bool almost_zero(T a, double e) 56 | { 57 | return (a == T(0)) || (a > 0 && a < e) || (a < 0 && a > -e); 58 | } 59 | 60 | struct color_rgb24 61 | { 62 | uint8_t r, g, b; 63 | color_rgb24(uint8_t R, uint8_t G, uint8_t B) 64 | : r(R), g(G), b(B) {} 65 | }; 66 | 67 | template 68 | struct vec3d 69 | { 70 | T x, y, z; 71 | 72 | vec3d() 73 | : x(0), y(0), z(0) {} 74 | vec3d(T x_, T y_, T z_) 75 | : x(x_), y(y_), z(z_) {} 76 | 77 | template 78 | vec3d(const vec3d & s) 79 | : x(T(s.x)), y(T(s.y)), z(T(s.z)) {} 80 | 81 | template 82 | explicit vec3d(const S * s) 83 | : x(T(s[0])), y(T(s[1])), z(T(s[2])) {} 84 | 85 | vec3d operator+(const vec3d & p) const 86 | { 87 | return vec3d(x + p.x, y + p.y, z + p.z); 88 | } 89 | 90 | vec3d operator-(const vec3d & p) const 91 | { 92 | return vec3d(x - p.x, y - p.y, z - p.z); 93 | } 94 | 95 | vec3d operator-() const {return vec3d(-x, -y, -z);} 96 | 97 | template 98 | vec3d & operator+=(const vec3d & p) 99 | { 100 | x += T(p.x); 101 | y += T(p.y); 102 | z += T(p.z); 103 | return *this; 104 | } 105 | 106 | // rules for partial ordering of function templates say that the new overload 107 | // is a better match, when it matches 108 | 109 | vec3d & operator+=(const vec3d & p) 110 | { 111 | x += p.x; 112 | y += p.y; 113 | z += p.z; 114 | return *this; 115 | } 116 | 117 | template 118 | vec3d & operator-=(const vec3d & p) 119 | { 120 | x -= T(p.x); 121 | y -= T(p.y); 122 | z -= T(p.z); 123 | return *this; 124 | } 125 | 126 | vec3d & operator-=(const vec3d & p) 127 | { 128 | x -= p.x; 129 | y -= p.y; 130 | z -= p.z; 131 | return *this; 132 | } 133 | 134 | template 135 | vec3d & operator/=(const Scalar & s) 136 | { 137 | const T i = T(1) / T(s); 138 | x *= i; 139 | y *= i; 140 | z *= i; 141 | return *this; 142 | } 143 | 144 | template 145 | vec3d & operator*=(const Scalar & s) 146 | { 147 | x *= T(s); 148 | y *= T(s); 149 | z *= T(s); 150 | return *this; 151 | } 152 | 153 | bool operator==(const vec3d & o) const 154 | { 155 | return (x == o.x) && (y == o.y) && (z == o.z); 156 | } 157 | 158 | template 159 | bool operator==(const vec3d & o) const 160 | { 161 | return x == T(o.x) && y == T(o.y) && z == T(o.z); 162 | } 163 | 164 | bool operator!=(const vec3d & o) const {return !(*this == o);} 165 | 166 | template 167 | bool operator!=(const vec3d & o) const 168 | { 169 | return !(*this == o); 170 | } 171 | 172 | template 173 | friend vec3d operator*(const vec3d & p, const Scalar & s) 174 | { 175 | return vec3d(s * p.x, s * p.y, s * p.z); 176 | } 177 | 178 | template 179 | friend vec3d operator*(const Scalar & s, const vec3d & p) 180 | { 181 | return p * s; 182 | } 183 | 184 | template 185 | friend vec3d operator/(const vec3d & p, const Scalar & s) 186 | { 187 | return vec3d(p.x / T(s), p.y / T(s), p.z / T(s)); 188 | } 189 | 190 | friend std::ostream & operator<<(std::ostream & os, const vec3d & p) 191 | { 192 | return os << p.x << " " << p.y << " " << p.z; 193 | } 194 | 195 | friend std::istream & operator>>(std::istream & is, vec3d & p) 196 | { 197 | return is >> p.x >> p.y >> p.z; 198 | } 199 | }; 200 | 201 | typedef vec3d normal3d; 202 | typedef vec3d point3d; 203 | 204 | class oriented_point3d : public point3d 205 | { 206 | public: 207 | normal3d n; 208 | 209 | oriented_point3d() 210 | : point3d() {} 211 | explicit oriented_point3d(const point3d & p) 212 | : point3d(p) {} 213 | oriented_point3d(double xx, double yy, double zz) 214 | : point3d(xx, yy, zz) {} 215 | oriented_point3d(const oriented_point3d & p) 216 | : point3d(p), n(p.n) {} 217 | oriented_point3d(const point3d & p, const normal3d & nn) 218 | : point3d(p), n(nn) {} 219 | }; 220 | 221 | struct triangle 222 | { 223 | oriented_point3d p0, p1, p2; 224 | int id0, id1, id2; // indices to vertices p0, p1, p2 225 | normal3d n; 226 | triangle() 227 | : id0(-1), id1(-1), id2(-1) {} 228 | triangle(int _id0, int _id1, int _id2) 229 | : id0(_id0), id1(_id1), id2(_id2) {} 230 | triangle( 231 | const oriented_point3d & p0_, const oriented_point3d & p1_, 232 | const oriented_point3d & p2_, const normal3d & n_) 233 | : p0(p0_), p1(p1_), p2(p2_), n(n_) {} 234 | triangle( 235 | const oriented_point3d & p0_, const oriented_point3d & p1_, 236 | const oriented_point3d & p2_, const normal3d & n_, int id0_, int id1_, 237 | int id2_) 238 | : p0(p0_), p1(p1_), p2(p2_), id0(id0_), id1(id1_), id2(id2_), n(n_) {} 239 | triangle( 240 | const point3d & p0_, const point3d & p1_, const point3d & p2_, 241 | const normal3d & n_) 242 | : p0(p0_), p1(p1_), p2(p2_), n(n_) {} 243 | friend std::ostream & operator<<(std::ostream & o, const triangle & t) 244 | { 245 | o << t.p0 << " " << t.p1 << " " << t.p2; 246 | return o; 247 | } 248 | }; 249 | 250 | class invalid_vector : public std::logic_error 251 | { 252 | public: 253 | invalid_vector() 254 | : std::logic_error("Exception invalid_vector caught.") {} 255 | explicit invalid_vector(const std::string & msg) 256 | : std::logic_error("Exception invalid_vector caught: " + msg) {} 257 | }; 258 | 259 | // ============================================================== 260 | // Rotations 261 | // ============================================================== 262 | 263 | // NOTE: this is a std::vector derivation, thus a matrix will 264 | // take 1 bit per element. 265 | 266 | template 267 | class matrix : private std::vector // row-major order 268 | { 269 | private: 270 | typedef std::vector super; 271 | int width_; 272 | int height_; 273 | 274 | public: 275 | const int & width; 276 | const int & height; 277 | 278 | typedef typename super::iterator iterator; 279 | typedef typename super::const_iterator const_iterator; 280 | 281 | matrix() 282 | : super(), width_(0), height_(0), width(width_), height(height_) {} 283 | 284 | matrix(int w, int h) 285 | : super(w * h), width_(w), height_(h), width(width_), height(height_) {} 286 | 287 | matrix(int w, int h, const T & v) 288 | : super(w * h, v), width_(w), height_(h), width(width_), height(height_) 289 | { 290 | } 291 | 292 | matrix(const matrix & m) 293 | : super(), width(width_), height(height_) 294 | { 295 | resize(m.width_, m.height_); 296 | super::assign(m.begin(), m.end()); 297 | } 298 | 299 | typename super::reference operator()(size_t r, size_t c) 300 | { 301 | return super::operator[](r * width_ + c); 302 | } 303 | typename super::const_reference operator()(size_t r, size_t c) const 304 | { 305 | return super::operator[](r * width_ + c); 306 | } 307 | 308 | typename super::reference at(const size_t r, const size_t c) 309 | { 310 | return super::at(r * width_ + c); 311 | } 312 | typename super::const_reference at(size_t r, size_t c) const 313 | { 314 | return super::at(r * width_ + c); 315 | } 316 | 317 | const T * to_ptr() const 318 | { 319 | return &(super::operator[]( 320 | 0)); // ok since std::vector is guaranteed to be contiguous 321 | } 322 | 323 | T * to_ptr() {return &(super::operator[](0));} 324 | 325 | void resize(int w, int h) 326 | { 327 | super::resize(w * h); 328 | width_ = w; 329 | height_ = h; 330 | } 331 | 332 | size_t size() const {return super::size();} 333 | 334 | iterator begin() {return super::begin();} 335 | const_iterator begin() const {return super::begin();} 336 | iterator end() {return super::end();} 337 | const_iterator end() const {return super::end();} 338 | 339 | bool operator==(const matrix & m) const 340 | { 341 | if ((width_ != m.width_) || (height != m.height_)) { 342 | return false; 343 | } 344 | 345 | const_iterator it1(begin()), it2(m.begin()), it1_end(end()); 346 | 347 | for (; it1 != it1_end; ++it1, ++it2) { 348 | if (*it1 != *it2) { 349 | return false; 350 | } 351 | } 352 | 353 | return true; 354 | } 355 | 356 | bool operator!=(const matrix & m) const {return !(*this == m);} 357 | 358 | matrix & operator=(const matrix & m) 359 | { 360 | if (&m == this) { 361 | return *this; 362 | } 363 | 364 | if (width != m.width || height != m.height) { 365 | throw invalid_vector("Cannot assign matrices with different sizes."); 366 | } 367 | 368 | super::assign(m.begin(), m.end()); 369 | return *this; 370 | } 371 | 372 | template 373 | matrix & operator*=(const S & s) 374 | { 375 | for (size_t i = 0; i < size(); ++i) { 376 | super::operator[](i) *= T(s); 377 | } 378 | return *this; 379 | } 380 | 381 | template 382 | matrix & operator/=(const S & s) 383 | { 384 | for (size_t i = 0; i < size(); ++i) { 385 | super::operator[](i) /= T(s); 386 | } 387 | return *this; 388 | } 389 | 390 | friend std::ostream & operator<<(std::ostream & s, const matrix & m) 391 | { 392 | for (int y = 0; y < m.height_; ++y) { 393 | for (int x = 0; x < m.width_; ++x) { 394 | s << m[y * m.width_ + x] << " "; 395 | } 396 | s << std::endl; 397 | } 398 | return s; 399 | } 400 | }; 401 | 402 | template 403 | struct matrix3x3 404 | { 405 | T r00, r01, r02, r10, r11, r12, r20, r21, r22; 406 | 407 | int width, height; 408 | 409 | matrix3x3() 410 | : r00(0), r01(0), r02(0), r10(0), r11(0), r12(0), r20(0), r21(0), r22(0), 411 | width(3), height(3) {} 412 | 413 | template 414 | explicit matrix3x3(const S * v) 415 | : r00(v[0]), r01(v[1]), r02(v[2]), r10(v[3]), r11(v[4]), r12(v[5]), 416 | r20(v[6]), r21(v[7]), r22(v[8]), width(3), height(3) {} 417 | 418 | void set_column(size_t c, const vec3d & v) 419 | { 420 | T x = v.x; 421 | T y = v.y; 422 | T z = v.z; 423 | 424 | if (c == 0) { 425 | r00 = x; 426 | r10 = y; 427 | r20 = z; 428 | } else if (c == 1) { 429 | r01 = x; 430 | r11 = y; 431 | r21 = z; 432 | } else if (c == 2) { 433 | r02 = x; 434 | r12 = y; 435 | r22 = z; 436 | } else { 437 | throw std::logic_error("Cannot set column for 3x3 matrix."); 438 | } 439 | } 440 | 441 | T & operator()(size_t row, size_t col) 442 | { 443 | switch (row) { 444 | case 0: 445 | if (col == 0) { 446 | return r00; 447 | } 448 | if (col == 1) { 449 | return r01; 450 | } 451 | if (col == 2) { 452 | return r02; 453 | } else { 454 | throw std::out_of_range("Cannot access element in 3x3 matrix"); 455 | } 456 | case 1: 457 | if (col == 0) { 458 | return r10; 459 | } 460 | if (col == 1) { 461 | return r11; 462 | } 463 | if (col == 2) { 464 | return r12; 465 | } else { 466 | throw std::out_of_range("Cannot access element in 3x3 matrix"); 467 | } 468 | case 2: 469 | if (col == 0) { 470 | return r20; 471 | } 472 | if (col == 1) { 473 | return r21; 474 | } 475 | if (col == 2) { 476 | return r22; 477 | } else { 478 | throw std::out_of_range("Cannot access element in 3x3 matrix"); 479 | } 480 | default: 481 | throw std::out_of_range("Cannot access element in 3x3 matrix"); 482 | } 483 | } 484 | 485 | const T & operator()(size_t row, size_t col) const 486 | { 487 | switch (row) { 488 | case 0: 489 | if (col == 0) { 490 | return r00; 491 | } 492 | if (col == 1) { 493 | return r01; 494 | } 495 | if (col == 2) { 496 | return r02; 497 | } else { 498 | throw std::out_of_range("Cannot access element in 3x3 matrix"); 499 | } 500 | case 1: 501 | if (col == 0) { 502 | return r10; 503 | } 504 | if (col == 1) { 505 | return r11; 506 | } 507 | if (col == 2) { 508 | return r12; 509 | } else { 510 | throw std::out_of_range("Cannot access element in 3x3 matrix"); 511 | } 512 | case 2: 513 | if (col == 0) { 514 | return r20; 515 | } 516 | if (col == 1) { 517 | return r21; 518 | } 519 | if (col == 2) { 520 | return r22; 521 | } else { 522 | throw std::out_of_range("Cannot access element in 3x3 matrix"); 523 | } 524 | default: 525 | throw std::out_of_range("Cannot access element in 3x3 matrix"); 526 | } 527 | } 528 | 529 | friend std::ostream & operator<<(std::ostream & s, const matrix3x3 & m) 530 | { 531 | s << m.r00 << " " << m.r01 << " " << m.r02 << std::endl; 532 | s << m.r10 << " " << m.r11 << " " << m.r12 << std::endl; 533 | s << m.r20 << " " << m.r21 << " " << m.r22 << std::endl; 534 | return s; 535 | } 536 | }; 537 | 538 | template 539 | inline matrix3x3 identity3x3() 540 | { 541 | matrix3x3 m; 542 | set_identity(m); 543 | return m; 544 | } 545 | 546 | template 547 | inline void mult_matrix_inplace( 548 | const matrix3x3 & m1, const matrix3x3 & m2, 549 | matrix3x3 & r) 550 | { 551 | const double r00 = m1.r00 * m2.r00 + m1.r01 * m2.r10 + m1.r02 * m2.r20; 552 | const double r01 = m1.r00 * m2.r01 + m1.r01 * m2.r11 + m1.r02 * m2.r21; 553 | const double r02 = m1.r00 * m2.r02 + m1.r01 * m2.r12 + m1.r02 * m2.r22; 554 | 555 | const double r10 = m1.r10 * m2.r00 + m1.r11 * m2.r10 + m1.r12 * m2.r20; 556 | const double r11 = m1.r10 * m2.r01 + m1.r11 * m2.r11 + m1.r12 * m2.r21; 557 | const double r12 = m1.r10 * m2.r02 + m1.r11 * m2.r12 + m1.r12 * m2.r22; 558 | 559 | const double r20 = m1.r20 * m2.r00 + m1.r21 * m2.r10 + m1.r22 * m2.r20; 560 | const double r21 = m1.r20 * m2.r01 + m1.r21 * m2.r11 + m1.r22 * m2.r21; 561 | const double r22 = m1.r20 * m2.r02 + m1.r21 * m2.r12 + m1.r22 * m2.r22; 562 | 563 | r.r00 = r00; 564 | r.r01 = r01; 565 | r.r02 = r02; 566 | r.r10 = r10; 567 | r.r11 = r11; 568 | r.r12 = r12; 569 | r.r20 = r20; 570 | r.r21 = r21; 571 | r.r22 = r22; 572 | } 573 | 574 | template 575 | inline void mult_matrix( 576 | const matrix3x3 & m1, const matrix3x3 & m2, 577 | matrix3x3 & r) 578 | { 579 | if (&r == &m1 || &r == &m2) { 580 | throw std::logic_error("math3d::mult_matrix() argument alias"); 581 | } 582 | return mult_matrix_inplace(m1, m2, r); 583 | } 584 | 585 | // NO in-place! 586 | template 587 | void mult_matrix(const Rot1 & m1, const Rot2 & m2, Rot3 & r) 588 | { 589 | if ((char *)&r == (char *)&m1 || (char *)&r == (char *)&m2) { 590 | throw std::logic_error("math3d::mult_matrix() argument alias"); 591 | } 592 | 593 | if (m1.width != m2.height || r.height != m1.height || r.width != m2.width) { 594 | throw std::logic_error("Incompatible size matrices"); 595 | } 596 | 597 | double sum; 598 | 599 | for (int is = 0; is < m1.height; ++is) { 600 | for (int jd = 0; jd < m2.width; ++jd) { 601 | sum = 0.; 602 | for (int js = 0; js < m1.width; ++js) { 603 | sum += m1(is, js) * m2(js, jd); 604 | } 605 | r(is, jd) = sum; 606 | } 607 | } 608 | } 609 | 610 | // ============================================================== 611 | // Quaternions 612 | // ============================================================== 613 | 614 | template 615 | struct quaternion 616 | { 617 | T w, i, j, k; 618 | 619 | explicit quaternion(T v = 0) 620 | : w(v), i(0), j(0), k(0) {} 621 | // explicit quaternion(const T* p) : w(p[0]), i(p[1]), j(p[2]), k(p[3]) {} 622 | quaternion(T ww, T ii, T jj, T kk) 623 | : w(ww), i(ii), j(jj), k(kk) {} 624 | 625 | static quaternion convert(const vec3d & p) 626 | { 627 | return quaternion(0, p.x, p.y, p.z); 628 | } 629 | 630 | static quaternion convert(const T * p) 631 | { 632 | return quaternion(p[0], p[1], p[2], p[3]); 633 | } 634 | 635 | quaternion & operator+=(const quaternion & a) 636 | { 637 | w += a.w; 638 | i += a.i; 639 | j += a.j; 640 | k += a.k; 641 | return *this; 642 | } 643 | 644 | quaternion & operator*=(T a) 645 | { 646 | w *= a; 647 | i *= a; 648 | j *= a; 649 | k *= a; 650 | return *this; 651 | } 652 | 653 | void to_vector(T * p) const 654 | { 655 | p[0] = w; 656 | p[1] = i; 657 | p[2] = j; 658 | p[3] = k; 659 | } 660 | 661 | friend std::ostream & operator<<(std::ostream & os, const quaternion & q) 662 | { 663 | return os << "[ " << q.w << " " << q.i << " " << q.j << " " << q.k << " ]"; 664 | } 665 | 666 | friend std::istream & operator>>(std::istream & is, quaternion & q) 667 | { 668 | std::string dump; 669 | return is >> dump >> q.w >> q.i >> q.j >> q.k >> dump; 670 | } 671 | }; 672 | 673 | template 674 | quaternion operator+(const quaternion & a, const quaternion & b) 675 | { 676 | quaternion result(a); 677 | result += b; 678 | return result; 679 | } 680 | 681 | template 682 | T dot(const quaternion & a, const quaternion & b) 683 | { 684 | return a.w * b.w + a.i * b.i + a.j * b.j + a.k * b.k; 685 | } 686 | 687 | template 688 | T norm(const quaternion & a) 689 | { 690 | return std::sqrt(dot(a, a)); 691 | } 692 | 693 | template 694 | quaternion operator*(const quaternion & a, const quaternion & b) 695 | { 696 | quaternion result; 697 | 698 | result.w = a.w * b.w - a.i * b.i - a.j * b.j - a.k * b.k; 699 | result.i = a.i * b.w + a.w * b.i - a.k * b.j + a.j * b.k; 700 | result.j = a.j * b.w + a.k * b.i + a.w * b.j - a.i * b.k; 701 | result.k = a.k * b.w - a.j * b.i + a.i * b.j + a.w * b.k; 702 | 703 | return result; 704 | } 705 | 706 | template 707 | quaternion operator~(const quaternion & a) 708 | { 709 | return quaternion(a.w, -a.i, -a.j, -a.k); 710 | } 711 | 712 | template 713 | inline void conjugate(quaternion & q) 714 | { 715 | q.i = -q.i; 716 | q.j = -q.j; 717 | q.k = -q.k; 718 | } 719 | 720 | template 721 | inline void normalize(quaternion & q) 722 | { 723 | T mag = q.w * q.w + q.i * q.i + q.j * q.j + q.k * q.k; 724 | if (!almost_zero(mag - T(1), 1e-10)) { 725 | mag = std::sqrt(mag); 726 | q.w /= mag; 727 | q.i /= mag; 728 | q.j /= mag; 729 | q.k /= mag; 730 | } 731 | } 732 | 733 | template 734 | inline void set_identity(quaternion & q) 735 | { 736 | q.w = T(1); 737 | q.i = q.j = q.k = T(0); 738 | } 739 | 740 | // returns a normalized unit quaternion 741 | template 742 | quaternion rot_matrix_to_quaternion(const matrix3x3 & m) 743 | { 744 | const T m00 = m(0, 0); 745 | const T m11 = m(1, 1); 746 | const T m22 = m(2, 2); 747 | const T m01 = m(0, 1); 748 | const T m02 = m(0, 2); 749 | const T m10 = m(1, 0); 750 | const T m12 = m(1, 2); 751 | const T m20 = m(2, 0); 752 | const T m21 = m(2, 1); 753 | const T tr = m00 + m11 + m22; 754 | 755 | quaternion ret; 756 | 757 | if (tr > 0) { 758 | T s = std::sqrt(tr + T(1)) * 2; // S=4*qw 759 | ret.w = 0.25 * s; 760 | ret.i = (m21 - m12) / s; 761 | ret.j = (m02 - m20) / s; 762 | ret.k = (m10 - m01) / s; 763 | } else if ((m00 > m11) & (m00 > m22)) { 764 | const T s = std::sqrt(T(1) + m00 - m11 - m22) * 2; // S=4*qx 765 | ret.w = (m21 - m12) / s; 766 | ret.i = 0.25 * s; 767 | ret.j = (m01 + m10) / s; 768 | ret.k = (m02 + m20) / s; 769 | } else if (m11 > m22) { 770 | const T s = std::sqrt(T(1) + m11 - m00 - m22) * 2; // S=4*qy 771 | ret.w = (m02 - m20) / s; 772 | ret.i = (m01 + m10) / s; 773 | ret.j = 0.25 * s; 774 | ret.k = (m12 + m21) / s; 775 | } else { 776 | const T s = std::sqrt(T(1) + m22 - m00 - m11) * 2; // S=4*qz 777 | ret.w = (m10 - m01) / s; 778 | ret.i = (m02 + m20) / s; 779 | ret.j = (m12 + m21) / s; 780 | ret.k = 0.25 * s; 781 | } 782 | 783 | return ret; 784 | } 785 | 786 | // assumes a normalized unit quaternion 787 | template 788 | matrix3x3 quaternion_to_rot_matrix(const quaternion & q) 789 | { 790 | matrix3x3 m; 791 | const T w = q.w, i = q.i, j = q.j, k = q.k; 792 | 793 | m(0, 0) = 1 - 2 * j * j - 2 * k * k; 794 | m(0, 1) = 2 * i * j - 2 * k * w; 795 | m(0, 2) = 2 * i * k + 2 * j * w; 796 | 797 | m(1, 0) = 2 * i * j + 2 * k * w; 798 | m(1, 1) = 1 - 2 * i * i - 2 * k * k; 799 | m(1, 2) = 2 * j * k - 2 * i * w; 800 | 801 | m(2, 0) = 2 * i * k - 2 * j * w; 802 | m(2, 1) = 2 * j * k + 2 * i * w; 803 | m(2, 2) = 1 - 2 * i * i - 2 * j * j; 804 | 805 | return m; 806 | } 807 | 808 | template 809 | inline void mult_quaternion( 810 | const quaternion & a, const quaternion & b, 811 | quaternion & r) 812 | { 813 | r.w = a.w * b.w - (a.i * b.i + a.j * b.j + a.k * b.k); 814 | r.i = a.w * b.i + b.w * a.i + a.j * b.k - a.k * b.j; 815 | r.j = a.w * b.j + b.w * a.j + a.k * b.i - a.i * b.k; 816 | r.k = a.w * b.k + b.w * a.k + a.i * b.j - a.j * b.i; 817 | } 818 | 819 | // ============================================================== 820 | // 821 | // ============================================================== 822 | 823 | template 824 | void transpose(matrix & m) 825 | { 826 | matrix old(m); 827 | const int w = m.width; 828 | const int h = m.height; 829 | m.resize(h, w); 830 | 831 | for (int row = 0; row < h; ++row) { 832 | for (int col = 0; col < w; ++col) { 833 | m(col, row) = old(row, col); 834 | } 835 | } 836 | } 837 | 838 | template 839 | inline void transpose(matrix3x3 & m) 840 | { 841 | const T m01 = m.r01, m02 = m.r02, m12 = m.r12, m10 = m.r10, m21 = m.r21, 842 | m20 = m.r20; 843 | m.r01 = m10; 844 | m.r02 = m20; 845 | m.r10 = m01; 846 | m.r20 = m02; 847 | m.r12 = m21; 848 | m.r21 = m12; 849 | } 850 | 851 | template 852 | inline matrix3x3 get_transpose(const matrix3x3 & m) 853 | { 854 | matrix3x3 ret; 855 | ret.r00 = m.r00; 856 | ret.r01 = m.r10; 857 | ret.r02 = m.r20; 858 | ret.r10 = m.r01; 859 | ret.r11 = m.r11; 860 | ret.r20 = m.r02; 861 | ret.r12 = m.r21; 862 | ret.r21 = m.r12; 863 | ret.r22 = m.r22; 864 | return ret; 865 | } 866 | 867 | // dest matrix must already be of the right size 868 | template 869 | void transpose(const matrix & src, matrix & dest) 870 | { 871 | if (src.width != dest.height || src.height != dest.width) { 872 | throw math3d::invalid_vector( 873 | "math3d::transpose(): Destination matrix must be of the right size."); 874 | } 875 | 876 | const int w = src.width; 877 | const int h = src.height; 878 | 879 | for (int row = 0; row < h; ++row) { 880 | for (int col = 0; col < w; ++col) { 881 | dest(col, row) = src(row, col); 882 | } 883 | } 884 | } 885 | 886 | template 887 | inline void transpose(const matrix3x3 & src, matrix3x3 & dest) 888 | { 889 | dest.r00 = src.r00; 890 | dest.r11 = src.r11; 891 | dest.r22 = src.r22; 892 | dest.r01 = src.r10; 893 | dest.r02 = src.r20; 894 | dest.r10 = src.r01; 895 | dest.r12 = src.r21; 896 | dest.r21 = src.r12; 897 | dest.r20 = src.r02; 898 | } 899 | 900 | template 901 | void set_identity(matrix & m, T val = 1) 902 | { 903 | if (m.width != m.height) { 904 | throw invalid_vector("Cannot set identity on a rectangular matrix."); 905 | } 906 | 907 | if (m.width == 0) { 908 | return; 909 | } 910 | 911 | const int n = m.width * m.height; 912 | const int w = m.width; 913 | 914 | int one = 0; 915 | for (int k = 0; k < n; ++k) { 916 | if (k == one) { 917 | m(k / w, k % w) = val; 918 | one += w + 1; 919 | } else { 920 | m(k / w, k % w) = 0; 921 | } 922 | } 923 | } 924 | 925 | template 926 | void set_identity(matrix3x3 & m, T val = 1) 927 | { 928 | m.r00 = val; 929 | m.r01 = 0; 930 | m.r02 = 0; 931 | m.r10 = 0; 932 | m.r11 = val; 933 | m.r12 = 0; 934 | m.r20 = 0; 935 | m.r21 = 0; 936 | m.r22 = val; 937 | } 938 | 939 | // ============================================================== 940 | // Rigid Motion 941 | // ============================================================== 942 | 943 | typedef std::pair, point3d> rigid_motion_t; 944 | 945 | template 946 | void rotate(vec3d & p, const matrix & rot) 947 | { 948 | if (rot.height != 3 || rot.width != 3) { 949 | throw std::logic_error("Rotation matrix must be 3x3"); 950 | } 951 | 952 | T oldx = p.x, oldy = p.y, oldz = p.z; 953 | p.x = oldx * rot(0, 0) + oldy * rot(0, 1) + oldz * rot(0, 2); 954 | p.y = oldx * rot(1, 0) + oldy * rot(1, 1) + oldz * rot(1, 2); 955 | p.z = oldx * rot(2, 0) + oldy * rot(2, 1) + oldz * rot(2, 2); 956 | } 957 | 958 | template 959 | void rotate(vec3d & p, const matrix3x3 & rot) 960 | { 961 | T oldx = p.x, oldy = p.y, oldz = p.z; 962 | p.x = oldx * rot.r00 + oldy * rot.r01 + oldz * rot.r02; 963 | p.y = oldx * rot.r10 + oldy * rot.r11 + oldz * rot.r12; 964 | p.z = oldx * rot.r20 + oldy * rot.r21 + oldz * rot.r22; 965 | } 966 | 967 | template 968 | void rotate(vec3d & p, const matrix & rot) 969 | { 970 | if (rot.height != 3 || rot.width != 3) { 971 | throw std::logic_error("Rotation matrix must be 3x3"); 972 | } 973 | 974 | T oldx = p.x, oldy = p.y, oldz = p.z; 975 | p.x = T(oldx * rot(0, 0) + oldy * rot(0, 1) + oldz * rot(0, 2)); 976 | p.y = T(oldx * rot(1, 0) + oldy * rot(1, 1) + oldz * rot(1, 2)); 977 | p.z = T(oldx * rot(2, 0) + oldy * rot(2, 1) + oldz * rot(2, 2)); 978 | } 979 | 980 | template 981 | inline void rotate(vec3d & p, const matrix3x3 & rot) 982 | { 983 | T oldx = p.x, oldy = p.y, oldz = p.z; 984 | p.x = T(oldx * rot.r00 + oldy * rot.r01 + oldz * rot.r02); 985 | p.y = T(oldx * rot.r10 + oldy * rot.r11 + oldz * rot.r12); 986 | p.z = T(oldx * rot.r20 + oldy * rot.r21 + oldz * rot.r22); 987 | } 988 | 989 | template 990 | inline void rotate(vec3d & p, const quaternion & rot) 991 | { 992 | rotate(p, quaternion_to_rot_matrix(rot)); 993 | } 994 | 995 | template 996 | inline vec3d get_rotate(const vec3d & v, const quaternion & q) 997 | { 998 | return get_rotate(v, quaternion_to_rot_matrix(q)); 999 | /* 1000 | const T 1001 | a = q.w, b = q.i, c = q.j, d = q.k, 1002 | t2 = a*b, t3 = a*c, t4 = a*d, t5 = -b*b, t6 = b*c, 1003 | t7 = b*d, t8 = -c*c, t9 = c*d, t10 = -d*d, 1004 | v1 = v.x, v2 = v.y, v3 = v.z; 1005 | return vec3d( 1006 | 2*( (t8 + t10)*v1 + (t6 - t4)*v2 + (t3 + t7)*v3 ) + v1, 1007 | 2*( (t4 + t6)*v1 + (t5 + t10)*v2 + (t9 - t2)*v3 ) + v2, 1008 | 2*( (t7 - t3)*v1 + (t2 + t9)*v2 + (t5 + t8)*v3 ) + v3); 1009 | */ 1010 | } 1011 | 1012 | template 1013 | inline vec3d get_rotate(const vec3d & p, const matrix3x3 & rot) 1014 | { 1015 | return vec3d( 1016 | p.x * rot.r00 + p.y * rot.r01 + p.z * rot.r02, 1017 | p.x * rot.r10 + p.y * rot.r11 + p.z * rot.r12, 1018 | p.x * rot.r20 + p.y * rot.r21 + p.z * rot.r22); 1019 | } 1020 | 1021 | template 1022 | inline void rotate_translate( 1023 | vec3d & v, const RotationType & rot, 1024 | const point3d & trans) 1025 | { 1026 | rotate(v, rot); 1027 | v += trans; 1028 | } 1029 | 1030 | template 1031 | inline vec3d get_rotate_translate( 1032 | const vec3d & p, const matrix3x3 & rot, 1033 | const point3d & t) 1034 | { 1035 | return vec3d( 1036 | p.x * rot.r00 + p.y * rot.r01 + p.z * rot.r02 + t.x, 1037 | p.x * rot.r10 + p.y * rot.r11 + p.z * rot.r12 + t.y, 1038 | p.x * rot.r20 + p.y * rot.r21 + p.z * rot.r22 + t.z); 1039 | } 1040 | 1041 | template 1042 | inline vec3d get_rotate_translate( 1043 | const vec3d & p, const matrix & rot, 1044 | const point3d & t) 1045 | { 1046 | if (rot.height != 3 || rot.width != 3) { 1047 | throw std::logic_error("Rotation matrix must be 3x3"); 1048 | } 1049 | 1050 | return vec3d( 1051 | p.x * rot(0, 0) + p.y * rot(0, 1) + p.z * rot(0, 2) + t.x, 1052 | p.x * rot(1, 0) + p.y * rot(1, 1) + p.z * rot(1, 2) + t.y, 1053 | p.x * rot(2, 0) + p.y * rot(2, 1) + p.z * rot(2, 2) + t.z); 1054 | } 1055 | 1056 | template 1057 | inline vec3d get_rotate_translate( 1058 | const vec3d & p, const T * rot, 1059 | const T * t) 1060 | { 1061 | return get_rotate_translate(p, matrix3x3(rot), vec3d(t[0], t[1], t[2])); 1062 | } 1063 | 1064 | template 1065 | inline vec3d get_rotate_translate( 1066 | const vec3d & v, 1067 | const quaternion & rot, 1068 | const point3d & t) 1069 | { 1070 | return get_rotate(v, rot) + t; 1071 | } 1072 | 1073 | /** 1074 | * Inverts a rigid motion. 1075 | */ 1076 | template 1077 | inline void invert(R & r, T & t) 1078 | { 1079 | transpose(r); 1080 | rotate(t, r); 1081 | t.x = -t.x; 1082 | t.y = -t.y; 1083 | t.z = -t.z; 1084 | } 1085 | 1086 | /** 1087 | * Computes the rigid motion bringing points expressed in j-coordinates 1088 | * towards the world i, i.e.: Pi = Rij * Pj + Tij 1089 | */ 1090 | template 1091 | void relative_motion( 1092 | 1093 | const matrix3x3 & Ri, const point3d & Ti, const matrix3x3 & Rj, 1094 | const point3d & Tj, matrix3x3 & Rij, point3d & Tij 1095 | 1096 | ) 1097 | { 1098 | matrix3x3 Ri_inv = Ri; 1099 | point3d Ti_inv = Ti; 1100 | invert(Ri_inv, Ti_inv); 1101 | 1102 | mult_matrix(Ri_inv, Rj, Rij); 1103 | Tij = get_rotate_translate(Tj, Ri_inv, Ti_inv); 1104 | } 1105 | 1106 | // ============================================================== 1107 | // Vector Operations 1108 | // ============================================================== 1109 | 1110 | template 1111 | inline double normalize(vec3d & p) 1112 | { 1113 | const double n = magnitude(p); 1114 | if (n == 0.) { 1115 | p.x = 0; 1116 | p.y = 0; 1117 | p.z = 0; 1118 | } else { 1119 | p.x /= n; 1120 | p.y /= n; 1121 | p.z /= n; 1122 | } 1123 | return n; 1124 | } 1125 | 1126 | template 1127 | inline vec3d get_normalize(const vec3d & p) 1128 | { 1129 | vec3d q(p); 1130 | normalize(q); 1131 | return q; 1132 | } 1133 | 1134 | template 1135 | inline double dist(const T & p1, const T & p2) 1136 | { 1137 | const double sqdist = squared_dist(p1, p2); 1138 | return sqdist == 0. ? 0. : std::sqrt(sqdist); 1139 | } 1140 | 1141 | // ||p1 - p2||^2 1142 | template 1143 | inline double squared_dist(const vec3d & p1, const vec3d & p2) 1144 | { 1145 | T x = p1.x - p2.x; 1146 | T y = p1.y - p2.y; 1147 | T z = p1.z - p2.z; 1148 | return (x * x) + (y * y) + (z * z); 1149 | } 1150 | 1151 | template 1152 | inline T dot_product(const vec3d & v1, const vec3d & v2) 1153 | { 1154 | return (v1.x * v2.x) + (v1.y * v2.y) + (v1.z * v2.z); 1155 | } 1156 | 1157 | template 1158 | inline double dot_product(const vec3d & v1, const vec3d & v2) 1159 | { 1160 | return (v1.x * v2.x) + (v1.y * v2.y) + (v1.z * v2.z); 1161 | } 1162 | 1163 | template 1164 | inline T dot_product(const quaternion & p, const quaternion & q) 1165 | { 1166 | return p.w * q.w + p.i * q.i + p.j * q.j + p.k * q.k; 1167 | } 1168 | 1169 | template 1170 | inline double norm2(const T & v) 1171 | { 1172 | return dot_product(v, v); 1173 | } 1174 | 1175 | template 1176 | inline double magnitude(const T & p) 1177 | { 1178 | return std::sqrt(dot_product(p, p)); 1179 | } 1180 | 1181 | template 1182 | inline vec3d cross_product(const vec3d & v1, const vec3d & v2) 1183 | { 1184 | return vec3d( 1185 | (v1.y * v2.z) - (v1.z * v2.y), (v1.z * v2.x) - (v1.x * v2.z), 1186 | (v1.x * v2.y) - (v1.y * v2.x)); 1187 | } 1188 | 1189 | template 1190 | inline double median(Iterator start, Iterator end) 1191 | { 1192 | const typename Iterator::difference_type n = end - start; 1193 | if (n <= 0) { 1194 | return 0.; 1195 | } 1196 | 1197 | if (n % 2 == 0) { 1198 | return (*(start + (n / 2)) + *(start + (n / 2 - 1))) / 2.; 1199 | } else { 1200 | return *(start + ((n - 1) / 2)); 1201 | } 1202 | } 1203 | 1204 | } // namespace math3d 1205 | 1206 | #endif // TRAC_IK__MATH3D_H_ 1207 | --------------------------------------------------------------------------------