diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 000000000..175111fa0 --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,10 @@ +version: 2 +updates: + - package-ecosystem: "github-actions" + directory: "/" + schedule: + interval: "daily" + - package-ecosystem: "gitsubmodule" + directory: "/" + schedule: + interval: "daily" diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml new file mode 100644 index 000000000..0dc629bbb --- /dev/null +++ b/.github/workflows/main.yml @@ -0,0 +1,106 @@ +name: CI + +on: [push, pull_request, workflow_dispatch] + +env: + CXXFLAGS: "-Wall -Wextra -Wno-unused-parameter" + +jobs: + build: + name: Build + runs-on: ${{ matrix.os }} + strategy: + fail-fast: false + matrix: + os: [ubuntu-20.04, ubuntu-22.04] + orocos_build_type: [Debug, Release] + compiler: [gcc, clang] + python_version: ['3.8', '3.10'] + exclude: + - os: ubuntu-20.04 + python_version: '3.10' + - os: ubuntu-22.04 + python_version: '3.8' + include: + - os: ubuntu-20.04 + orocos_build_type: Release + compiler: gcc + python_version: '3.9' + - os: ubuntu-20.04 + orocos_build_type: Release + compiler: gcc + python_version: '3.10' + - os: ubuntu-20.04 + orocos_build_type: Release + compiler: gcc + python_version: '3.11' + - os: ubuntu-22.04 + orocos_build_type: Release + compiler: gcc + python_version: '3.11' + env: + CC: ${{ matrix.compiler }} + OROCOS_BUILD_TYPE: ${{ matrix.orocos_build_type }} + ROS_PYTHON_VERSION: ${{ matrix.python_version }} + steps: + - uses: actions/checkout@v4 + with: + submodules: recursive + - uses: actions/setup-python@v4 + with: + python-version: ${{ matrix.python_version }} + - name: Install + run: | + sudo apt-get install libeigen3-dev libcppunit-dev + pip install psutil future + - name: Build orocos_kdl + run: | + cd orocos_kdl + mkdir build + cd build + cmake -DENABLE_TESTS:BOOL=ON -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} ./.. + make + sudo make install + - name: Build PyKDL + run: | + cd python_orocos_kdl + mkdir build + cd build + cmake -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} ./.. + make + sudo make install + - name: ldconfig + run: sudo ldconfig + - name: Test orocos_kdl + run: | + cd orocos_kdl/build + make check + - name: Test PyKDL + run: | + cd python_orocos_kdl + python_version_short=$(python -c "import sys; print('{}.{}'.format(sys.version_info[0], sys.version_info[1]))") + export PYTHONPATH=/usr/local/lib/python${python_version_short}/dist-packages${PYTHONPATH:+:${PYTHONPATH}} + python tests/PyKDLtest.py + + industrial_ci: + name: Industrial CI - ${{ matrix.env.ROS_DISTRO }} + runs-on: ubuntu-latest + strategy: + fail-fast: false + matrix: + include: + - env: + ROS_DISTRO: noetic + ROS_REPO: ros + ABICHECK_URL: github:orocos/orocos_kinematics_dynamics#release-1.5 + ABICHECK_MERGE: false + branch: release-1.5 + env: ${{ matrix.env }} + steps: + - uses: actions/checkout@v4 + with: + submodules: recursive + if: ${{ (github.event_name == 'push' && endsWith(github.ref, matrix.branch)) || (github.event_name == 'pull_request' && endsWith(github.base_ref, matrix.branch)) }} + - uses: ros-industrial/industrial_ci@master + env: ${{ matrix.env }} + if: ${{ (github.event_name == 'push' && endsWith(github.ref, matrix.branch)) || (github.event_name == 'pull_request' && endsWith(github.base_ref, matrix.branch)) }} diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 000000000..8fdde6858 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,4 @@ +[submodule "python_orocos_kdl/pybind11"] + path = python_orocos_kdl/pybind11 + url = https://github.com/pybind/pybind11.git + branch = stable diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index 8d8a697c6..000000000 --- a/.travis.yml +++ /dev/null @@ -1,41 +0,0 @@ -language: cpp - -compiler: - - gcc - - clang -env: - - OROCOS_KDL_BUILD_TYPE=Debug - - OROCOS_KDL_BUILD_TYPE=Release - -before_script: - - sudo apt-get -qq update - - sudo apt-get install -y libeigen3-dev libcppunit-dev python-sip-dev - #build orocos_kdl - - cd orocos_kdl - - mkdir build - - cd build - - cmake -DENABLE_TESTS:BOOL=ON -DCMAKE_CXX_FLAGS:STRING="-Wall" -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} ./.. - # compile and install orocos_kdl - - make - - sudo make install - - cd ../.. - #build python bindings - - cd python_orocos_kdl - - mkdir build - - cd build - - cmake -DCMAKE_CXX_FLAGS:STRING="-Wall" -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} ./.. - #compile and install python bindings - - make - - sudo make install - - export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib - - sudo ldconfig - - cd ../.. - -script: - #execute orocos_kdl tests - - cd orocos_kdl/build - - make check - - cd ../.. - #execute python bindings tests - - cd python_orocos_kdl/build - - python ../tests/PyKDLtest.py diff --git a/README.md b/README.md new file mode 100644 index 000000000..4c18ea7ec --- /dev/null +++ b/README.md @@ -0,0 +1,20 @@ +# Kinematics and Dynamics Library + +[![CI](https://github.com/orocos/orocos_kinematics_dynamics/workflows/CI/badge.svg)](https://github.com/orocos/orocos_kinematics_dynamics/actions) + +Orocos project to supply RealTime usable kinematics and dynamics code, +it contains code for rigid body kinematics calculations and +representations for kinematic structures and their inverse and forward +kinematic solvers. + +The C++ library is located in the `orocos_kdl` folder. The installation instructions can be found in +[INSTALL.md](orocos_kdl/INSTALL.md). + +The python bindings, are located in the `python_orocos_kdl` folder. The installation instructions can be found in +[INSTALL.md](python_orocos_kdl/INSTALL.md). + +Always use the same version of the C++ library and the python bindings. As a mismatch between these two can cause many issues. + +Also when using ROS/catkin, it is preferred to use the catkin installation method over the `cmake/make` method. + +There will be no ROS Noetic release. diff --git a/orocos_kdl/CMakeLists.txt b/orocos_kdl/CMakeLists.txt index c323e145a..f9d37e02d 100644 --- a/orocos_kdl/CMakeLists.txt +++ b/orocos_kdl/CMakeLists.txt @@ -1,8 +1,7 @@ # # Test CMake version # -CMAKE_MINIMUM_REQUIRED(VERSION 2.6) -#MARK_AS_ADVANCED( FORCE CMAKE_BACKWARDS_COMPATIBILITY ) +CMAKE_MINIMUM_REQUIRED(VERSION 3.0.2) ################################################### @@ -13,7 +12,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6) PROJECT(orocos_kdl) -SET( KDL_VERSION 1.4.0) +SET( KDL_VERSION 1.5.1) STRING( REGEX MATCHALL "[0-9]+" KDL_VERSIONS ${KDL_VERSION} ) LIST( GET KDL_VERSIONS 0 KDL_VERSION_MAJOR) LIST( GET KDL_VERSIONS 1 KDL_VERSION_MINOR) @@ -24,6 +23,14 @@ MESSAGE( STATUS "Orocos KDL version ${VERSION} (${KDL_VERSION_MAJOR}.${KDL_VERSI SET( PROJ_SOURCE_DIR ${orocos_kdl_SOURCE_DIR} ) SET( PROJ_BINARY_DIR ${orocos_kdl_BINARY_DIR} ) +# catkin-specific configuration (optional) +find_package(catkin QUIET) +if(catkin_FOUND) + catkin_package( + SKIP_CMAKE_CONFIG_GENERATION + SKIP_PKG_CONFIG_GENERATION + ) +endif() IF(NOT CMAKE_INSTALL_PREFIX) SET( CMAKE_INSTALL_PREFIX /usr/local/ CACHE PATH "Installation directory" FORCE) @@ -42,38 +49,40 @@ ENDIF ( NOT CMAKE_BUILD_TYPE ) SET( KDL_CFLAGS "") -find_package(Eigen 3 QUIET) -if(NOT Eigen_FOUND) - include(${PROJ_SOURCE_DIR}/config/FindEigen3.cmake) - set(Eigen_INCLUDE_DIR "${EIGEN3_INCLUDE_DIR}") +find_package(Eigen3 QUIET) +if(NOT EIGEN3_FOUND) + include(${PROJ_SOURCE_DIR}/cmake/FindEigen3.cmake) endif() -include_directories(${Eigen_INCLUDE_DIR}) -SET(KDL_CFLAGS "${KDL_CFLAGS} -I${Eigen_INCLUDE_DIR}") +include_directories(${EIGEN3_INCLUDE_DIR}) +SET(KDL_CFLAGS "${KDL_CFLAGS} -I\"${EIGEN3_INCLUDE_DIR}\"") # Check the platform STL containers capabilities -include(config/CheckSTLContainers.cmake) +include(cmake/CheckSTLContainers.cmake) CHECK_STL_CONTAINERS() # Set the default option appropriately if(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) - set(KDL_USE_NEW_TREE_INTERFACE_DEFAULT Off) + set(KDL_USE_NEW_TREE_INTERFACE_DEFAULT OFF) else(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) - set(KDL_USE_NEW_TREE_INTERFACE_DEFAULT On) + set(KDL_USE_NEW_TREE_INTERFACE_DEFAULT ON) endif(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) # Allow the user to select the Tree API version to use set(KDL_USE_NEW_TREE_INTERFACE ${KDL_USE_NEW_TREE_INTERFACE_DEFAULT} CACHE BOOL "Use the new KDL Tree interface") +#Sanity check, inform the user +if(NOT HAVE_STL_CONTAINER_INCOMPLETE_TYPES AND NOT KDL_USE_NEW_TREE_INTERFACE) + message(WARNING "You have chosen to use the current Tree Interface, but your platform doesn't support containers of " + "incomplete types, this configuration is likely invalid") +endif() + # The new interface requires the use of shared pointers if(KDL_USE_NEW_TREE_INTERFACE) # We need shared_ptr from boost since not all compilers are c++11 capable find_package(Boost REQUIRED) include_directories(${Boost_INCLUDE_DIRS}) - set(KDL_INCLUDE_DIRS ${Boost_INCLUDE_DIRS}) endif(KDL_USE_NEW_TREE_INTERFACE) -INCLUDE (${PROJ_SOURCE_DIR}/config/DependentOption.cmake) - OPTION(ENABLE_TESTS OFF "Enable building of tests") IF( ENABLE_TESTS ) # If not in standard paths, set CMAKE_xxx_PATH's in environment, eg. @@ -103,15 +112,20 @@ export(TARGETS orocos-kdl export(PACKAGE orocos_kdl) -set(KDL_INCLUDE_DIRS ${KDL_INCLUDE_DIRS} ${Eigen_INCLUDE_DIR}) - -CONFIGURE_FILE(KDLConfig.cmake.in +# Generate CMake package configuration +CONFIGURE_FILE(orocos_kdl-config.cmake.in ${PROJECT_BINARY_DIR}/orocos_kdl-config.cmake @ONLY) -CONFIGURE_FILE(KDLConfigVersion.cmake.in +CONFIGURE_FILE(orocos_kdl-config-version.cmake.in ${PROJECT_BINARY_DIR}/orocos_kdl-config-version.cmake @ONLY) +INSTALL(FILES cmake/FindEigen3.cmake DESTINATION share/orocos_kdl/cmake) INSTALL(FILES ${PROJECT_BINARY_DIR}/orocos_kdl-config.cmake DESTINATION share/orocos_kdl/cmake) INSTALL(FILES ${PROJECT_BINARY_DIR}/orocos_kdl-config-version.cmake DESTINATION share/orocos_kdl/cmake) INSTALL(EXPORT OrocosKDLTargets DESTINATION share/orocos_kdl/cmake) -INSTALL(FILES package.xml DESTINATION share/orocos_kdl) +# Generate pkg-config package configuration +CONFIGURE_FILE(orocos_kdl.pc.in ${CMAKE_CURRENT_BINARY_DIR}/orocos-kdl.pc @ONLY) +CONFIGURE_FILE(orocos_kdl.pc.in ${CMAKE_CURRENT_BINARY_DIR}/orocos_kdl.pc @ONLY) + +INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/orocos-kdl.pc DESTINATION lib${LIB_SUFFIX}/pkgconfig) +INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/orocos_kdl.pc DESTINATION lib${LIB_SUFFIX}/pkgconfig) diff --git a/orocos_kdl/INSTALL b/orocos_kdl/INSTALL deleted file mode 100644 index e56bc0e47..000000000 --- a/orocos_kdl/INSTALL +++ /dev/null @@ -1,16 +0,0 @@ -1. create a new build-directory - (it is always better not to build in the - src) - mkdir -2. cd -3. ccmake .. -4. adapt CMAKE_INSTALL_PREFIX to desired installation directory -5. If you want to build the tests: enable BUILD_TESTS -6. If you want to build the python bindings: enable PYTHON_BINDING, you need sip 4.5 for this to work -7 Generate (press g) -8. make -9. To execute the tests: make check -10. To create to API-documentation: make docs (in ) -11. To install: make install [DESTDIR=...] -12.To uninstall: make uninstall - \ No newline at end of file diff --git a/orocos_kdl/INSTALL.md b/orocos_kdl/INSTALL.md new file mode 100644 index 000000000..c46a8f626 --- /dev/null +++ b/orocos_kdl/INSTALL.md @@ -0,0 +1,37 @@ +# Install instructions + +These install instructions are focused on Debian/Ubuntu systems. + +## Shared instructions + +1. (Optional) Update apt cache: `sudo apt-get update` +2. Install EIGEN and cppunit: `sudo apt-get install libeigen3-dev libcppunit-dev` +3. (Optional) Install `Doxygen` and `Graphviz` to generate API-documentation: `sudo apt-get install doxygen graphviz` + +## Compilation + +### With catkin + +1. Clone the repository inside the workspace +2. Build with your catkin tool of preference +3. Source the workspace +4. (Optional) To generate the API-documentation use either [rosdoc_lite](http://wiki.ros.org/rosdoc_lite) or +[catkin_tools_document](https://github.com/mikepurvis/catkin_tools_document) + +### Without catkin + +1. Clone the repository where you want +2. Go to the `orocos_kdl` folder": `cd orocos_kdl` +3. Create a new build folder (it is always better not to build in the source folder): `mkdir build` +4. Go to the build folder `cd build` +5. Execute cmake: `cmake ..` + - (Optional) Adapt `CMAKE_INSTALL_PREFIX` to the desired installation directory + - (Optional) To build the tests, add: `-DENABLE_TESTS:BOOL=ON` + - (Optional) To change the build type, add: `-DCMAKE_BUILD_TYPE=` +6. Compile: `make` +7. Install the library: `sudo make install` +8. (Optional) To execute the tests: `make check` +9. (Optional) To create the API-documentation: `make docs`. The API-documentation will be generated at +`/doc/api/html`. + +To uninstall the library: `sudo make uninstall` diff --git a/orocos_kdl/KDLConfig.cmake.in b/orocos_kdl/KDLConfig.cmake.in deleted file mode 100644 index 3c3097c0d..000000000 --- a/orocos_kdl/KDLConfig.cmake.in +++ /dev/null @@ -1,17 +0,0 @@ -# - Config file for the orocos-kdl package -# It defines the following variables -# orocos_kdl_INCLUDE_DIRS - include directories for Orocos KDL -# orocos_kdl_LIBRARIES - libraries to link against for Orocos KDL -# orocos_kdl_PKGCONFIG_DIR - directory containing the .pc pkgconfig files - -# Compute paths -set(orocos_kdl_INCLUDE_DIRS "${CMAKE_CURRENT_LIST_DIR}/../../../include;@Boost_INCLUDE_DIRS@;@Eigen_INCLUDE_DIR@") - -if(NOT TARGET orocos-kdl) - include("${CMAKE_CURRENT_LIST_DIR}/OrocosKDLTargets.cmake") -endif() - -set(orocos_kdl_LIBRARIES orocos-kdl) - -# where the .pc pkgconfig files are installed -set(orocos_kdl_PKGCONFIG_DIR "${CMAKE_CURRENT_LIST_DIR}/../../../lib/pkgconfig") diff --git a/orocos_kdl/README b/orocos_kdl/README deleted file mode 100644 index b6c557c2f..000000000 --- a/orocos_kdl/README +++ /dev/null @@ -1,8 +0,0 @@ -Kinematics and Dynamics Library: - -Orocos project to supply RealTime usable kinematics and dynamics code, -it contains code for rigid body kinematics calculations and -representations for kinematic structures and their inverse and forward -kinematic solvers. - - diff --git a/orocos_kdl/config/CheckSTLContainers.cmake b/orocos_kdl/cmake/CheckSTLContainers.cmake similarity index 100% rename from orocos_kdl/config/CheckSTLContainers.cmake rename to orocos_kdl/cmake/CheckSTLContainers.cmake diff --git a/orocos_kdl/config/FindEigen3.cmake b/orocos_kdl/cmake/FindEigen3.cmake similarity index 100% rename from orocos_kdl/config/FindEigen3.cmake rename to orocos_kdl/cmake/FindEigen3.cmake diff --git a/orocos_kdl/config/FindPkgConfig.cmake b/orocos_kdl/cmake/FindPkgConfig.cmake similarity index 100% rename from orocos_kdl/config/FindPkgConfig.cmake rename to orocos_kdl/cmake/FindPkgConfig.cmake diff --git a/orocos_kdl/config/DependentOption.cmake b/orocos_kdl/config/DependentOption.cmake deleted file mode 100644 index 4ec4a6e1b..000000000 --- a/orocos_kdl/config/DependentOption.cmake +++ /dev/null @@ -1,37 +0,0 @@ -# - Macro to provide an option dependent on other options. -# This macro presents an option to the user only if a set of other -# conditions are true. When the option is not presented a default -# value is used, but any value set by the user is preserved for when -# the option is presented again. -# Example invocation: -# DEPENDENT_OPTION(USE_FOO "Use Foo" ON -# "USE_BAR;NOT USE_ZOT" OFF) -# If USE_BAR is true and USE_ZOT is false, this provides an option called -# USE_FOO that defaults to ON. Otherwise, it sets USE_FOO to OFF. If -# the status of USE_BAR or USE_ZOT ever changes, any value for the -# USE_FOO option is saved so that when the option is re-enabled it -# retains its old value. -MACRO(DEPENDENT_OPTION option doc default depends force) - IF(${option}_ISSET MATCHES "^${option}_ISSET$") - SET(${option}_AVAILABLE 1) - FOREACH(d ${depends}) - STRING(REGEX REPLACE " +" ";" CMAKE_DEPENDENT_OPTION_DEP "${d}") - IF(${CMAKE_DEPENDENT_OPTION_DEP}) - ELSE(${CMAKE_DEPENDENT_OPTION_DEP}) - SET(${option}_AVAILABLE 0) - ENDIF(${CMAKE_DEPENDENT_OPTION_DEP}) - ENDFOREACH(d) - IF(${option}_AVAILABLE) - OPTION(${option} "${doc}" "${default}") - SET(${option} "${${option}}" CACHE BOOL "${doc}" FORCE) - ELSE(${option}_AVAILABLE) - IF(${option} MATCHES "^${option}$") - ELSE(${option} MATCHES "^${option}$") - SET(${option} "${${option}}" CACHE INTERNAL "${doc}") - ENDIF(${option} MATCHES "^${option}$") - SET(${option} ${force}) - ENDIF(${option}_AVAILABLE) - ELSE(${option}_ISSET MATCHES "^${option}_ISSET$") - SET(${option} "${${option}_ISSET}") - ENDIF(${option}_ISSET MATCHES "^${option}_ISSET$") -ENDMACRO(DEPENDENT_OPTION) diff --git a/orocos_kdl/debian/copyright b/orocos_kdl/debian/copyright index b7463ac97..d4c898e17 100644 --- a/orocos_kdl/debian/copyright +++ b/orocos_kdl/debian/copyright @@ -8,19 +8,19 @@ Copyright Holder: Orocos Dev Team License: - This program is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; either version 2 of the License, or - (at your option) any later version. + This program is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. - You should have received a copy of the GNU General Public License - along with this package; if not, write to the Free Software - Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + You should have received a copy of the GNU Lesser General Public License + along with this package; if not, write to the Free Software Foundation, + Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. -On Debian systems, the complete text of the GNU General -Public License can be found in `/usr/share/common-licenses/GPL'. +On Debian systems, the complete text of the GNU Lesser General +Public License can be found in `/usr/share/common-licenses/LGPL'. diff --git a/orocos_kdl/doc/tex/UserManual.tex b/orocos_kdl/doc/tex/UserManual.tex index 784b0ad1a..4accacc2a 100644 --- a/orocos_kdl/doc/tex/UserManual.tex +++ b/orocos_kdl/doc/tex/UserManual.tex @@ -120,7 +120,7 @@ \subsection{Vector} \paragraph{Add and subtract vectors} \label{sec:add-subtract-vectors} -You can add or substract a vector with another vector: +You can add or subtract a vector with another vector: \begin{lstlisting} v2+=v1; v3-=v1; diff --git a/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp b/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp index f2121d4d3..55ffe7e72 100644 --- a/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp +++ b/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp @@ -54,6 +54,8 @@ estimate of shortest time per invposkin (ms) 0.155544 #include #include #include +#include + #include /** @@ -62,134 +64,130 @@ estimate of shortest time per invposkin (ms) 0.155544 * \TODO provide other examples. */ void test_inverseposkin(KDL::Chain& chain) { - using namespace KDL; - using namespace std; - boost::timer timer; - int num_of_trials = 1000000; - int total_number_of_iter = 0; - int n = chain.getNrOfJoints(); - int nrofresult_ok = 0; - int nrofresult_minus1=0; - int nrofresult_minus2=0; - int nrofresult_minus3=0; - int min_num_of_iter = 10000000; - int max_num_of_iter = 0; - double min_diff = 1E10; - double max_diff = 0; - double min_trans_diff = 1E10; - double max_trans_diff = 0; - double min_rot_diff = 1E10; - double max_rot_diff = 0; - Eigen::Matrix L; - L(0)=1;L(1)=1;L(2)=1; - L(3)=0.01;L(4)=0.01;L(5)=0.01; - ChainFkSolverPos_recursive fwdkin(chain); - ChainIkSolverPos_LMA solver(chain,L); - JntArray q(n); - JntArray q_init(n); - JntArray q_sol(n); - for (int trial=0;trial max_num_of_iter) max_num_of_iter = solver.lastNrOfIter; - if (solver.lastNrOfIter < min_num_of_iter) min_num_of_iter = solver.lastNrOfIter; - if (retval!=-3) { - if (solver.lastDifference > max_diff) max_diff = solver.lastDifference; - if (solver.lastDifference < min_diff) min_diff = solver.lastDifference; - - if (solver.lastTransDiff > max_trans_diff) max_trans_diff = solver.lastTransDiff; - if (solver.lastTransDiff < min_trans_diff) min_trans_diff = solver.lastTransDiff; - - if (solver.lastRotDiff > max_trans_diff) max_rot_diff = solver.lastRotDiff; - if (solver.lastRotDiff < min_trans_diff) min_rot_diff = solver.lastRotDiff; - } - fwdkin.JntToCart(q_sol,pos_reached); - } - cout << "------------------ statistics ------------------------------"< L; + L(0)=1;L(1)=1;L(2)=1; + L(3)=0.01;L(4)=0.01;L(5)=0.01; + KDL::ChainFkSolverPos_recursive fwdkin(chain); + KDL::ChainIkSolverPos_LMA solver(chain,L); + KDL::JntArray q(n); + KDL::JntArray q_init(n); + KDL::JntArray q_sol(n); + for (int trial=0;trial max_num_of_iter) max_num_of_iter = solver.lastNrOfIter; + if (solver.lastNrOfIter < min_num_of_iter) min_num_of_iter = solver.lastNrOfIter; + if (retval!=-3) { + if (solver.lastDifference > max_diff) max_diff = solver.lastDifference; + if (solver.lastDifference < min_diff) min_diff = solver.lastDifference; + + if (solver.lastTransDiff > max_trans_diff) max_trans_diff = solver.lastTransDiff; + if (solver.lastTransDiff < min_trans_diff) min_trans_diff = solver.lastTransDiff; + + if (solver.lastRotDiff > max_trans_diff) max_rot_diff = solver.lastRotDiff; + if (solver.lastRotDiff < min_trans_diff) min_rot_diff = solver.lastRotDiff; + } + fwdkin.JntToCart(q_sol,pos_reached); + } + std::cout << "------------------ statistics ------------------------------"< #include +#include int main() { - //Creating Vectors KDL::Vector v1;//Default constructor KDL::Vector v2(1.0,2.0,3.0);//Most used constructor KDL::Vector v3(v2);//Copy constructor KDL::Vector v4 = KDL::Vector::Zero();//Static member - + //Use operator << to print the values of your vector std::cout<<"v1 ="< #include #include +#include #include int main(int argc,char* argv[]) { @@ -35,13 +36,13 @@ int main(int argc,char* argv[]) { // When the routines are parallel, no rounding is needed, and no attempt is made // add constructing a rounding arc. // (It is still not possible when the segments are on top of each other) - // Note that you can only rotate in a deterministic way over an angle less then M_PI! - // With an angle == M_PI, you cannot predict over which side will be rotated. - // With an angle > M_PI, the routine will rotate over 2*M_PI-angle. + // Note that you can only rotate in a deterministic way over an angle less then PI! + // With an angle == PI, you cannot predict over which side will be rotated. + // With an angle > PI, the routine will rotate over 2*PI-angle. // If you need to rotate over a larger angle, you need to introduce intermediate points. // So, there is a common use case for using parallel segments. - path->Add(Frame(Rotation::RPY(M_PI,0,0), Vector(-1,0,0))); - path->Add(Frame(Rotation::RPY(M_PI/2,0,0), Vector(-0.5,0,0))); + path->Add(Frame(Rotation::RPY(PI,0,0), Vector(-1,0,0))); + path->Add(Frame(Rotation::RPY(PI_2,0,0), Vector(-0.5,0,0))); path->Add(Frame(Rotation::RPY(0,0,0), Vector(0,0,0))); path->Add(Frame(Rotation::RPY(0.7,0.7,0.7), Vector(1,1,1))); path->Add(Frame(Rotation::RPY(0,0.7,0), Vector(1.5,0.3,0))); @@ -61,8 +62,6 @@ int main(int argc,char* argv[]) { ctraject->Add(traject); ctraject->Add(new Trajectory_Stationary(1.0,Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0)))); - - // use the trajectory double dt=0.1; std::ofstream of("./trajectory.dat"); @@ -113,5 +112,3 @@ int main(int argc,char* argv[]) { } } - - diff --git a/orocos_kdl/models/kukaLWRtestDHnew.cpp b/orocos_kdl/models/kukaLWRtestDHnew.cpp index 947fbeefc..f0dcedbcf 100644 --- a/orocos_kdl/models/kukaLWRtestDHnew.cpp +++ b/orocos_kdl/models/kukaLWRtestDHnew.cpp @@ -7,7 +7,8 @@ #include using namespace KDL; -using namespace std; + +using std::setw; void outputLine( double, double, double, double, double, double, double); int getInputs(JntArray&, JntArray&, JntArray&, int&); @@ -35,23 +36,23 @@ int main(int argc , char** argv){ std::cout<<"tau: "<> linenr; + std::cout << "Give experiment number= line number in files \n ?"; + std::cin >> linenr; //read files: code based on example 14.8, c++ how to program, Deitel and Deitel, book p 712 @@ -79,16 +80,16 @@ int getInputs(JntArray &_q, JntArray &_qdot, JntArray &_qdotdot, int &linenr) *READING Q = joint positions */ - ifstream inQfile("interpreteerbaar/q", ios::in); + std::ifstream inQfile("interpreteerbaar/q", std::ios::in); if (!inQfile) { - cerr << "File q could not be opened \n"; + std::cerr << "File q could not be opened \n"; exit(1); } //print headers - cout << setiosflags( ios::left) << setw(15) << "_q(0)" << setw(15) << "_q(1)" << setw(15) << "_q(2)" << setw(15) << "_q(3)" << setw(15) << "_q(4)" << setw(15) << "_q(5)" << setw(15) << "_q(6)" << " \n" ; + std::cout << setiosflags( std::ios::left) << setw(15) << "_q(0)" << setw(15) << "_q(1)" << setw(15) << "_q(2)" << setw(15) << "_q(3)" << setw(15) << "_q(4)" << setw(15) << "_q(5)" << setw(15) << "_q(6)" << " \n" ; while(!inQfile.eof()) { @@ -108,16 +109,16 @@ int getInputs(JntArray &_q, JntArray &_qdot, JntArray &_qdotdot, int &linenr) *READING Qdot = joint velocities */ counter=0;//reset counter - ifstream inQdotfile("interpreteerbaar/qdot", ios::in); + std::ifstream inQdotfile("interpreteerbaar/qdot", std::ios::in); if (!inQdotfile) { - cerr << "File qdot could not be opened \n"; + std::cerr << "File qdot could not be opened \n"; exit(1); } //print headers - cout << setiosflags( ios::left) << setw(15) << "_qdot(0)" << setw(15) << "_qdot(1)" << setw(15) << "_qdot(2)" << setw(15) << "_qdot(3)" << setw(15) << "_qdot(4)" << setw(15) << "_qdot(5)" << setw(15) << "_qdot(6)" << " \n" ; + std::cout << setiosflags( std::ios::left) << setw(15) << "_qdot(0)" << setw(15) << "_qdot(1)" << setw(15) << "_qdot(2)" << setw(15) << "_qdot(3)" << setw(15) << "_qdot(4)" << setw(15) << "_qdot(5)" << setw(15) << "_qdot(6)" << " \n" ; while(!inQdotfile.eof()) { @@ -137,16 +138,16 @@ int getInputs(JntArray &_q, JntArray &_qdot, JntArray &_qdotdot, int &linenr) *READING Qdotdot = joint accelerations */ counter=0;//reset counter - ifstream inQdotdotfile("interpreteerbaar/qddot", ios::in); + std::ifstream inQdotdotfile("interpreteerbaar/qddot", std::ios::in); if (!inQdotdotfile) { - cerr << "File qdotdot could not be opened \n"; + std::cerr << "File qdotdot could not be opened \n"; exit(1); } //print headers - cout << setiosflags( ios::left) << setw(15) << "_qdotdot(0)" << setw(15) << "_qdotdot(1)" << setw(15) << "_qdotdot(2)" << setw(15) << "_qdotdot(3)" << setw(15) << "_qdotdot(4)" << setw(15) << "_qdotdot(5)" << setw(15) << "_qdotdot(6)" << " \n" ; + std::cout << setiosflags( std::ios::left) << setw(15) << "_qdotdot(0)" << setw(15) << "_qdotdot(1)" << setw(15) << "_qdotdot(2)" << setw(15) << "_qdotdot(3)" << setw(15) << "_qdotdot(4)" << setw(15) << "_qdotdot(5)" << setw(15) << "_qdotdot(6)" << " \n" ; while(!inQdotdotfile.eof()) { @@ -168,6 +169,6 @@ int getInputs(JntArray &_q, JntArray &_qdot, JntArray &_qdotdot, int &linenr) void outputLine( double x1, double x2, double x3, double x4, double x5, double x6, double x7) { - cout << setiosflags(ios::left) << setiosflags(ios::fixed | ios::showpoint) < #include +#include + using namespace KDL; -using namespace std; + +using std::setw; void outputLine( double, double, double, double, double, double, double); int getInputs(JntArray&, JntArray&, JntArray&, int&); @@ -59,23 +62,23 @@ int main(int argc , char** argv){ std::cout<<"H= \n"<> linenr; + std::cout << "Give experiment number= line number in files \n ?"; + std::cin >> linenr; //read files: code based on example 14.8, c++ how to program, Deitel and Deitel, book p 712 @@ -103,16 +106,16 @@ int getInputs(JntArray &_q, JntArray &_qdot, JntArray &_qdotdot, int &linenr) *READING Q = joint positions */ - ifstream inQfile("interpreteerbaar/q", ios::in); + std::ifstream inQfile("interpreteerbaar/q", std::ios::in); if (!inQfile) { - cerr << "File q could not be opened \n"; + std::cerr << "File q could not be opened \n"; exit(1); } //print headers - cout << setiosflags( ios::left) << setw(15) << "_q(0)" << setw(15) << "_q(1)" << setw(15) << "_q(2)" << setw(15) << "_q(3)" << setw(15) << "_q(4)" << setw(15) << "_q(5)" << setw(15) << "_q(6)" << " \n" ; + std::cout << setiosflags(std::ios::left) << setw(15) << "_q(0)" << setw(15) << "_q(1)" << setw(15) << "_q(2)" << setw(15) << "_q(3)" << setw(15) << "_q(4)" << setw(15) << "_q(5)" << setw(15) << "_q(6)" << " \n" ; while(!inQfile.eof()) { @@ -132,16 +135,16 @@ int getInputs(JntArray &_q, JntArray &_qdot, JntArray &_qdotdot, int &linenr) *READING Qdot = joint velocities */ counter=0;//reset counter - ifstream inQdotfile("interpreteerbaar/qdot", ios::in); + std::ifstream inQdotfile("interpreteerbaar/qdot", std::ios::in); if (!inQdotfile) { - cerr << "File qdot could not be opened \n"; + std::cerr << "File qdot could not be opened \n"; exit(1); } //print headers - cout << setiosflags( ios::left) << setw(15) << "_qdot(0)" << setw(15) << "_qdot(1)" << setw(15) << "_qdot(2)" << setw(15) << "_qdot(3)" << setw(15) << "_qdot(4)" << setw(15) << "_qdot(5)" << setw(15) << "_qdot(6)" << " \n" ; + std::cout << setiosflags(std::ios::left) << setw(15) << "_qdot(0)" << setw(15) << "_qdot(1)" << setw(15) << "_qdot(2)" << setw(15) << "_qdot(3)" << setw(15) << "_qdot(4)" << setw(15) << "_qdot(5)" << setw(15) << "_qdot(6)" << " \n" ; while(!inQdotfile.eof()) { @@ -161,16 +164,16 @@ int getInputs(JntArray &_q, JntArray &_qdot, JntArray &_qdotdot, int &linenr) *READING Qdotdot = joint accelerations */ counter=0;//reset counter - ifstream inQdotdotfile("interpreteerbaar/qddot", ios::in); + std::ifstream inQdotdotfile("interpreteerbaar/qddot", std::ios::in); if (!inQdotdotfile) { - cerr << "File qdotdot could not be opened \n"; + std::cerr << "File qdotdot could not be opened \n"; exit(1); } //print headers - cout << setiosflags( ios::left) << setw(15) << "_qdotdot(0)" << setw(15) << "_qdotdot(1)" << setw(15) << "_qdotdot(2)" << setw(15) << "_qdotdot(3)" << setw(15) << "_qdotdot(4)" << setw(15) << "_qdotdot(5)" << setw(15) << "_qdotdot(6)" << " \n" ; + std::cout << setiosflags(std::ios::left) << setw(15) << "_qdotdot(0)" << setw(15) << "_qdotdot(1)" << setw(15) << "_qdotdot(2)" << setw(15) << "_qdotdot(3)" << setw(15) << "_qdotdot(4)" << setw(15) << "_qdotdot(5)" << setw(15) << "_qdotdot(6)" << " \n" ; while(!inQdotdotfile.eof()) { @@ -185,13 +188,12 @@ int getInputs(JntArray &_q, JntArray &_qdot, JntArray &_qdotdot, int &linenr) } inQdotdotfile.close(); - return 0; } void outputLine( double x1, double x2, double x3, double x4, double x5, double x6, double x7) { - cout << setiosflags(ios::left) << setiosflags(ios::fixed | ios::showpoint) < #include "models.hpp" +#include namespace KDL{ Chain Puma560(){ Chain puma560; puma560.addSegment(Segment()); puma560.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH(0.0,M_PI_2,0.0,0.0), + Frame::DH(0.0,PI_2,0.0,0.0), RigidBodyInertia(0,Vector::Zero(),RotationalInertia(0,0.35,0,0,0,0)))); puma560.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(0.4318,0.0,0.0,0.0), RigidBodyInertia(17.4,Vector(-.3638,.006,.2275),RotationalInertia(0.13,0.524,0.539,0,0,0)))); puma560.addSegment(Segment()); puma560.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH(0.0203,-M_PI_2,0.15005,0.0), + Frame::DH(0.0203,-PI_2,0.15005,0.0), RigidBodyInertia(4.8,Vector(-.0203,-.0141,.070),RotationalInertia(0.066,0.086,0.0125,0,0,0)))); puma560.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH(0.0,M_PI_2,0.4318,0.0), + Frame::DH(0.0,PI_2,0.4318,0.0), RigidBodyInertia(0.82,Vector(0,.019,0),RotationalInertia(1.8e-3,1.3e-3,1.8e-3,0,0,0)))); puma560.addSegment(Segment()); puma560.addSegment(Segment()); puma560.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH(0.0,-M_PI_2,0.0,0.0), + Frame::DH(0.0,-PI_2,0.0,0.0), RigidBodyInertia(0.34,Vector::Zero(),RotationalInertia(.3e-3,.4e-3,.3e-3,0,0,0)))); puma560.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(0.0,0.0,0.0,0.0), diff --git a/orocos_kdl/KDLConfigVersion.cmake.in b/orocos_kdl/orocos_kdl-config-version.cmake.in similarity index 100% rename from orocos_kdl/KDLConfigVersion.cmake.in rename to orocos_kdl/orocos_kdl-config-version.cmake.in diff --git a/orocos_kdl/orocos_kdl-config.cmake.in b/orocos_kdl/orocos_kdl-config.cmake.in new file mode 100644 index 000000000..d17d43349 --- /dev/null +++ b/orocos_kdl/orocos_kdl-config.cmake.in @@ -0,0 +1,34 @@ +# - Config file for the orocos-kdl package +# It defines the following variables +# orocos_kdl_INCLUDE_DIRS - include directories for Orocos KDL +# orocos_kdl_LIBRARIES - libraries to link against for Orocos KDL +# orocos_kdl_PKGCONFIG_DIR - directory containing the .pc pkgconfig files + +# Compute paths +get_filename_component(orocos_kdl_PREFIX "${CMAKE_CURRENT_LIST_DIR}/../../.." ABSOLUTE) + +# Find dependencies +find_package(Eigen3 QUIET) +if(NOT EIGEN3_FOUND) + include(${CMAKE_CURRENT_LIST_DIR}/FindEigen3.cmake) +endif() + +set(KDL_USE_NEW_TREE_INTERFACE @KDL_USE_NEW_TREE_INTERFACE@) +if(KDL_USE_NEW_TREE_INTERFACE) + find_package(Boost REQUIRED) +endif() + +if(NOT TARGET orocos-kdl) + include("${CMAKE_CURRENT_LIST_DIR}/OrocosKDLTargets.cmake") +endif() + +set(orocos_kdl_INCLUDE_DIRS + ${orocos_kdl_PREFIX}/include + ${Boost_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +) +set(orocos_kdl_LIBRARIES orocos-kdl) +set(orocos_kdl_TARGETS orocos-kdl) + +# where the .pc pkgconfig files are installed +set(orocos_kdl_PKGCONFIG_DIR "${orocos_kdl_PREFIX}/lib/pkgconfig") diff --git a/orocos_kdl/src/kdl.pc.in b/orocos_kdl/orocos_kdl.pc.in similarity index 77% rename from orocos_kdl/src/kdl.pc.in rename to orocos_kdl/orocos_kdl.pc.in index 49e2ab971..8a477e844 100644 --- a/orocos_kdl/src/kdl.pc.in +++ b/orocos_kdl/orocos_kdl.pc.in @@ -1,5 +1,5 @@ -prefix=@CMAKE_INSTALL_PREFIX@ -libdir=${prefix}/lib +prefix=${pcfiledir}/../.. +libdir=${prefix}/lib@LIB_SUFFIX@ includedir=${prefix}/include Name: orocos-kdl @@ -8,4 +8,3 @@ Requires: Version: @KDL_VERSION@ Libs: -L${libdir} -lorocos-kdl Cflags: -I${includedir} @KDL_CFLAGS@ - diff --git a/orocos_kdl/package.xml b/orocos_kdl/package.xml index cc8cfbf0b..fffdd7d3f 100644 --- a/orocos_kdl/package.xml +++ b/orocos_kdl/package.xml @@ -1,24 +1,32 @@ - + + + orocos_kdl - 1.4.0 + 1.5.1 This package contains a recent version of the Kinematics and Dynamics - Library (KDL), distributed by the Orocos Project. + Library (KDL), distributed by the Orocos Project. - Ruben Smits + Ruben Smits http://wiki.ros.org/orocos_kdl - LGPL + LGPL-2.1-or-later - cmake + catkin eigen - catkin - eigen - pkg-config + catkin + eigen + pkg-config cppunit + + doxygen + cmake + diff --git a/orocos_kdl/rosdoc.yaml b/orocos_kdl/rosdoc.yaml new file mode 100644 index 000000000..58107573d --- /dev/null +++ b/orocos_kdl/rosdoc.yaml @@ -0,0 +1,5 @@ +- builder: doxygen + name: C++ API + file_patterns: '*.cpp *.cxx *.h *.hpp *.inl' + tab_size: 4 + exclude_patterns: '*.svn* CMake*' diff --git a/orocos_kdl/src/CMakeLists.txt b/orocos_kdl/src/CMakeLists.txt index 079ca8ac1..954b8fc07 100644 --- a/orocos_kdl/src/CMakeLists.txt +++ b/orocos_kdl/src/CMakeLists.txt @@ -3,52 +3,6 @@ FILE( GLOB KDL_HPPS [^.]*.hpp [^.]*.inl) FILE( GLOB UTIL_HPPS utilities/[^.]*.h utilities/[^.]*.hpp) -INCLUDE(CheckCXXSourceCompiles) -SET(CMAKE_REQUIRED_FLAGS) -CHECK_CXX_SOURCE_COMPILES(" - #include - #include - #include - - class TreeElement; - typedef std::map SegmentMap; - - class TreeElement - { - TreeElement(const std::string& name): number(0) {} - - public: - int number; - SegmentMap::const_iterator parent; - std::vector children; - - static TreeElement Root(std::string& name) - { - return TreeElement(name); - } - }; - - int main() - { - return 0; - } - " - HAVE_STL_CONTAINER_INCOMPLETE_TYPES) - -if(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) - SET(KDL_USE_NEW_TREE_INTERFACE_DEFAULT Off) -ELSE(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) - SET(KDL_USE_NEW_TREE_INTERFACE_DEFAULT On) -ENDIF(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) - -SET(KDL_USE_NEW_TREE_INTERFACE ${KDL_USE_NEW_TREE_INTERFACE_DEFAULT} CACHE BOOL "Use the new KDL Tree interface") - -#Sanity check, inform the user -IF(NOT HAVE_STL_CONTAINER_INCOMPLETE_TYPES AND NOT KDL_USE_NEW_TREE_INTERFACE) - MESSAGE(WARNING "You have chosen to use the current Tree Interface, but your platform doesn't support containers of " - "incomplete types, this configuration is likely invalid") -ENDIF() - #In Windows (Visual Studio) it is necessary to specify the postfix #of the debug library name and no symbols are exported by kdl, #so it is necessary to compile it as a static library @@ -62,46 +16,44 @@ ENDIF(MSVC) CONFIGURE_FILE(config.h.in config.h @ONLY) #### Settings for rpath -IF(${CMAKE_MINIMUM_REQUIRED_VERSION} VERSION_GREATER "2.8.12") - MESSAGE(AUTHOR_WARNING "CMAKE_MINIMUM_REQUIRED_VERSION is now ${CMAKE_MINIMUM_REQUIRED_VERSION}. This check can be removed.") -ENDIF() -IF(NOT (CMAKE_VERSION VERSION_LESS 2.8.12)) - IF(NOT MSVC) - #add the option to disable RPATH - OPTION(OROCOSKDL_ENABLE_RPATH "Enable RPATH during installation" TRUE) - MARK_AS_ADVANCED(OROCOSKDL_ENABLE_RPATH) - ENDIF(NOT MSVC) - - IF(OROCOSKDL_ENABLE_RPATH) - #Configure RPATH - SET(CMAKE_MACOSX_RPATH TRUE) #enable RPATH on OSX. This also suppress warnings on CMake >= 3.0 - # when building, don't use the install RPATH already - # (but later on when installing) - SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) - #build directory by default is built with RPATH - SET(CMAKE_SKIP_BUILD_RPATH FALSE) - - #This is relative RPATH for libraries built in the same project - #I assume that the directory is - # - install_dir/something for binaries - # - install_dir/lib for libraries - LIST(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/lib" isSystemDir) - IF("${isSystemDir}" STREQUAL "-1") - FILE(RELATIVE_PATH _rel_path "${CMAKE_INSTALL_PREFIX}/bin" "${CMAKE_INSTALL_PREFIX}/lib") - IF (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") - SET(CMAKE_INSTALL_RPATH "@loader_path/${_rel_path}") - ELSE() - SET(CMAKE_INSTALL_RPATH "\$ORIGIN/${_rel_path}") - ENDIF() - ENDIF("${isSystemDir}" STREQUAL "-1") - # add the automatically determined parts of the RPATH - # which point to directories outside the build tree to the install RPATH - SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) #very important! - ENDIF() +IF(NOT MSVC) + #add the option to disable RPATH + OPTION(OROCOSKDL_ENABLE_RPATH "Enable RPATH during installation" TRUE) + MARK_AS_ADVANCED(OROCOSKDL_ENABLE_RPATH) +ENDIF(NOT MSVC) + +IF(OROCOSKDL_ENABLE_RPATH) + #Configure RPATH + SET(CMAKE_MACOSX_RPATH TRUE) #enable RPATH on OSX. This also suppress warnings on CMake >= 3.0 + # when building, don't use the install RPATH already + # (but later on when installing) + SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) + #build directory by default is built with RPATH + SET(CMAKE_SKIP_BUILD_RPATH FALSE) + + #This is relative RPATH for libraries built in the same project + #I assume that the directory is + # - install_dir/something for binaries + # - install_dir/lib for libraries + LIST(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/lib" isSystemDir) + IF("${isSystemDir}" STREQUAL "-1") + FILE(RELATIVE_PATH _rel_path "${CMAKE_INSTALL_PREFIX}/bin" "${CMAKE_INSTALL_PREFIX}/lib") + IF (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") + SET(CMAKE_INSTALL_RPATH "@loader_path/${_rel_path}") + ELSE() + SET(CMAKE_INSTALL_RPATH "\$ORIGIN/${_rel_path}") + ENDIF() + ENDIF("${isSystemDir}" STREQUAL "-1") + # add the automatically determined parts of the RPATH + # which point to directories outside the build tree to the install RPATH + SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) #very important! ENDIF() #####end RPATH -ADD_LIBRARY(orocos-kdl ${LIB_TYPE} ${KDL_SRCS} config.h) +ADD_LIBRARY(orocos-kdl ${LIB_TYPE} ${KDL_SRCS}) + +TARGET_INCLUDE_DIRECTORIES(orocos-kdl PUBLIC + "$") SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES SOVERSION "${KDL_VERSION_MAJOR}.${KDL_VERSION_MINOR}" @@ -111,36 +63,23 @@ SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES ) #### Settings for rpath disabled (back-compatibility) -IF(${CMAKE_MINIMUM_REQUIRED_VERSION} VERSION_GREATER "2.8.12") - MESSAGE(AUTHOR_WARNING "CMAKE_MINIMUM_REQUIRED_VERSION is now ${CMAKE_MINIMUM_REQUIRED_VERSION}. This check can be removed.") -ENDIF() -IF(CMAKE_VERSION VERSION_LESS 2.8.12) +IF(NOT OROCOSKDL_ENABLE_RPATH) SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib${LIB_SUFFIX}") -ELSE() - IF(NOT OROCOSKDL_ENABLE_RPATH) - SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES - INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib${LIB_SUFFIX}") - ENDIF() ENDIF() #####end RPATH # Needed so that the generated config.h can be used -INCLUDE_DIRECTORIES(${CMAKE_CURRENT_BINARY_DIR}) +TARGET_INCLUDE_DIRECTORIES(orocos-kdl PUBLIC + "$") TARGET_LINK_LIBRARIES(orocos-kdl ${Boost_LIBRARIES}) INSTALL(TARGETS orocos-kdl EXPORT OrocosKDLTargets ARCHIVE DESTINATION lib${LIB_SUFFIX} LIBRARY DESTINATION lib${LIB_SUFFIX} + RUNTIME DESTINATION bin PUBLIC_HEADER DESTINATION include/kdl ) INSTALL(FILES ${UTIL_HPPS} DESTINATION include/kdl/utilities) - -# Orocos convention: -CONFIGURE_FILE( kdl.pc.in ${CMAKE_CURRENT_BINARY_DIR}/orocos-kdl.pc @ONLY) -CONFIGURE_FILE( kdl.pc.in ${CMAKE_CURRENT_BINARY_DIR}/orocos_kdl.pc @ONLY) - -INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/orocos-kdl.pc DESTINATION lib${LIB_SUFFIX}/pkgconfig) -INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/orocos_kdl.pc DESTINATION lib${LIB_SUFFIX}/pkgconfig) diff --git a/orocos_kdl/src/articulatedbodyinertia.cpp b/orocos_kdl/src/articulatedbodyinertia.cpp index e4cdcb937..709096d63 100644 --- a/orocos_kdl/src/articulatedbodyinertia.cpp +++ b/orocos_kdl/src/articulatedbodyinertia.cpp @@ -23,14 +23,12 @@ #include -using namespace Eigen; - namespace KDL{ ArticulatedBodyInertia::ArticulatedBodyInertia(const RigidBodyInertia& rbi) { - this->M=Matrix3d::Identity()*rbi.m; - this->I=Map(rbi.I.data); + this->M=Eigen::Matrix3d::Identity()*rbi.m; + this->I=Eigen::Map(rbi.I.data); this->H << 0,-rbi.h[2],rbi.h[1], rbi.h[2],0,-rbi.h[0], -rbi.h[1],rbi.h[0],0; @@ -41,7 +39,7 @@ namespace KDL{ *this = RigidBodyInertia(m,c,Ic); } - ArticulatedBodyInertia::ArticulatedBodyInertia(const Matrix3d& M, const Matrix3d& H, const Matrix3d& I) + ArticulatedBodyInertia::ArticulatedBodyInertia(const Eigen::Matrix3d& M, const Eigen::Matrix3d& H, const Eigen::Matrix3d& I) { this->M=M; this->I=I; @@ -69,8 +67,8 @@ namespace KDL{ Wrench operator*(const ArticulatedBodyInertia& I,const Twist& t){ Wrench result; - Vector3d::Map(result.force.data)=I.M*Vector3d::Map(t.vel.data)+I.H.transpose()*Vector3d::Map(t.rot.data); - Vector3d::Map(result.torque.data)=I.I*Vector3d::Map(t.rot.data)+I.H*Vector3d::Map(t.vel.data); + Eigen::Vector3d::Map(result.force.data)=I.M*Eigen::Vector3d::Map(t.vel.data)+I.H.transpose()*Eigen::Vector3d::Map(t.rot.data); + Eigen::Vector3d::Map(result.torque.data)=I.I*Eigen::Vector3d::Map(t.rot.data)+I.H*Eigen::Vector3d::Map(t.vel.data); return result; } @@ -79,18 +77,18 @@ namespace KDL{ //mb=ma //hb=R*(h-m*r) //Ib = R(Ia+r x h x + (h-m*r) x r x)R' - Map E(X.M.data); - Matrix3d rcross; + Eigen::Map E(X.M.data); + Eigen::Matrix3d rcross; rcross << 0,-X.p[2],X.p[1], X.p[2],0,-X.p[0], -X.p[1],X.p[0],0; - Matrix3d HrM=I.H-rcross*I.M; + Eigen::Matrix3d HrM=I.H-rcross*I.M; return ArticulatedBodyInertia(E*I.M*E.transpose(),E*HrM*E.transpose(),E*(I.I-rcross*I.H.transpose()+HrM*rcross)*E.transpose()); } ArticulatedBodyInertia operator*(const Rotation& M,const ArticulatedBodyInertia& I){ - Map E(M.data); + Eigen::Map E(M.data); return ArticulatedBodyInertia(E.transpose()*I.M*E,E.transpose()*I.H*E,E.transpose()*I.I*E); } @@ -98,12 +96,12 @@ namespace KDL{ //mb=ma //hb=R*(h-m*r) //Ib = R(Ia+r x h x + (h-m*r) x r x)R' - Matrix3d rcross; + Eigen::Matrix3d rcross; rcross << 0,-p[2],p[1], p[2],0,-p[0], -p[1],p[0],0; - Matrix3d HrM=this->H-rcross*this->M; + Eigen::Matrix3d HrM=this->H-rcross*this->M; return ArticulatedBodyInertia(this->M,HrM,this->I-rcross*this->H.transpose()+HrM*rcross); } }//namespace diff --git a/orocos_kdl/src/chain.cpp b/orocos_kdl/src/chain.cpp index 2b6320b27..43eb4f144 100644 --- a/orocos_kdl/src/chain.cpp +++ b/orocos_kdl/src/chain.cpp @@ -1,8 +1,8 @@ -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2007 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -22,7 +22,6 @@ #include "chain.hpp" namespace KDL { - using namespace std; Chain::Chain(): nrOfJoints(0), @@ -55,7 +54,7 @@ namespace KDL { { segments.push_back(segment); nrOfSegments++; - if(segment.getJoint().getType()!=Joint::None) + if(segment.getJoint().getType()!=Joint::Fixed) nrOfJoints++; } diff --git a/orocos_kdl/src/chaindynparam.cpp b/orocos_kdl/src/chaindynparam.cpp index 3773c3b0f..f9f9fb90d 100644 --- a/orocos_kdl/src/chaindynparam.cpp +++ b/orocos_kdl/src/chaindynparam.cpp @@ -1,8 +1,9 @@ -// Copyright (C) 2009 Dominick Vanthienen +// Copyright (C) 2009 Dominick Vanthienen // Version: 1.0 -// Author: Dominick Vanthienen -// Maintainer: Ruben Smits +// Author: Dominick Vanthienen +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -65,13 +66,13 @@ namespace KDL { return (error = E_SIZE_MISMATCH); unsigned int k=0; double q_; - + //Sweep from root to leaf for(unsigned int i=0;i=0;i--) { - + if(i!=0) { //assumption that previous segment is parent Ic[i-1]=Ic[i-1]+X[i]*Ic[i]; - } + } F=Ic[i]*S[i]; - if(chain.getSegment(i).getJoint().getType()!=Joint::None) + if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) { - H(k,k)=dot(S[i],F); + H(k,k)=dot(S[i],F); + H(k,k)+=chain.getSegment(i).getJoint().getInertia(); // add joint inertia j=k; //countervariable for the joints l=i; //countervariable for the segments while(l!=0) //go from leaf to root starting at i @@ -106,14 +108,14 @@ namespace KDL { //assumption that previous segment is parent F=X[l]*F; //calculate the unit force (cfr S) for every segment: F[l-1]=X[l]*F[l] l--; //go down a segment - - if(chain.getSegment(l).getJoint().getType()!=Joint::None) //if the joint connected to segment is not a fixed joint - { + + if(chain.getSegment(l).getJoint().getType()!=Joint::Fixed) //if the joint connected to segment is not a fixed joint + { j--; - H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment + H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment H(j,k)=H(k,j); } - } + } k--; //this if-loop should be repeated nj times (k=nj-1 to k=0) } @@ -127,10 +129,10 @@ namespace KDL { //make a null matrix with the size of q_dotdot and a null wrench SetToZero(jntarraynull); - + //the calculation of coriolis matrix C return chainidsolver_coriolis.CartToJnt(q, q_dot, jntarraynull, wrenchnull, coriolis); - + } //calculate gravity matrix G @@ -138,7 +140,7 @@ namespace KDL { { //make a null matrix with the size of q_dotdot and a null wrench - + SetToZero(jntarraynull); //the calculation of coriolis matrix C return chainidsolver_gravity.CartToJnt(q, jntarraynull, jntarraynull, wrenchnull, gravity); diff --git a/orocos_kdl/src/chaindynparam.hpp b/orocos_kdl/src/chaindynparam.hpp index 964c213ae..38fd225d4 100644 --- a/orocos_kdl/src/chaindynparam.hpp +++ b/orocos_kdl/src/chaindynparam.hpp @@ -1,8 +1,8 @@ -// Copyright (C) 2009 Dominick Vanthienen +// Copyright (C) 2009 Dominick Vanthienen // Version: 1.0 -// Author: Dominick Vanthienen -// Maintainer: Ruben Smits +// Author: Dominick Vanthienen +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -44,7 +44,7 @@ namespace KDL { * (expressed in the segments reference frame) and the dynamical * parameters of the segments. */ - class ChainDynParam : SolverI + class ChainDynParam : public SolverI { public: ChainDynParam(const Chain& chain, Vector _grav); @@ -59,7 +59,7 @@ namespace KDL { private: const Chain& chain; - int nr; + int nr; // unused, remove in a future version unsigned int nj; unsigned int ns; Vector grav; diff --git a/orocos_kdl/src/chainexternalwrenchestimator.cpp b/orocos_kdl/src/chainexternalwrenchestimator.cpp new file mode 100644 index 000000000..8c9d7526f --- /dev/null +++ b/orocos_kdl/src/chainexternalwrenchestimator.cpp @@ -0,0 +1,214 @@ +// Copyright (C) 2021 Djordje Vukcevic + +// Version: 1.0 +// Author: Djordje Vukcevic +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "chainexternalwrenchestimator.hpp" + +namespace KDL { + +ChainExternalWrenchEstimator::ChainExternalWrenchEstimator(const Chain &chain, const Vector &gravity, const double sample_frequency, const double estimation_gain, const double filter_constant, const double eps, const int maxiter) : + CHAIN(chain), + DT_SEC(1.0 / sample_frequency), FILTER_CONST(filter_constant), + svd_eps(eps), + svd_maxiter(maxiter), + nj(CHAIN.getNrOfJoints()), ns(CHAIN.getNrOfSegments()), + jnt_mass_matrix(nj), previous_jnt_mass_matrix(nj), jnt_mass_matrix_dot(nj), + initial_jnt_momentum(nj), estimated_momentum_integral(nj), filtered_estimated_ext_torque(nj), + gravity_torque(nj), coriolis_torque(nj), total_torque(nj), estimated_ext_torque(nj), + jacobian_end_eff(nj), + jacobian_end_eff_transpose(Eigen::MatrixXd::Zero(nj, 6)), jacobian_end_eff_transpose_inv(Eigen::MatrixXd::Zero(6, nj)), + U(Eigen::MatrixXd::Zero(nj, 6)), V(Eigen::MatrixXd::Zero(6, 6)), + S(Eigen::VectorXd::Zero(6)), S_inv(Eigen::VectorXd::Zero(6)), tmp(Eigen::VectorXd::Zero(6)), + ESTIMATION_GAIN(Eigen::VectorXd::Constant(nj, estimation_gain)), + dynparam_solver(CHAIN, gravity), + jacobian_solver(CHAIN), + fk_pos_solver(CHAIN) +{ +} + +void ChainExternalWrenchEstimator::updateInternalDataStructures() +{ + nj = CHAIN.getNrOfJoints(); + ns = CHAIN.getNrOfSegments(); + jnt_mass_matrix.resize(nj); + previous_jnt_mass_matrix.resize(nj); + jnt_mass_matrix_dot.resize(nj); + initial_jnt_momentum.resize(nj); + estimated_momentum_integral.resize(nj); + filtered_estimated_ext_torque.resize(nj); + gravity_torque.resize(nj); + coriolis_torque.resize(nj); + total_torque.resize(nj); + estimated_ext_torque.resize(nj); + jacobian_end_eff.resize(nj); + jacobian_end_eff_transpose.conservativeResizeLike(Eigen::MatrixXd::Zero(nj, 6)); + jacobian_end_eff_transpose_inv.conservativeResizeLike(Eigen::MatrixXd::Zero(6, nj)); + U.conservativeResizeLike(Eigen::MatrixXd::Zero(nj, 6)); + V.conservativeResizeLike(Eigen::MatrixXd::Zero(6, 6)); + S.conservativeResizeLike(Eigen::VectorXd::Zero(6)); + S_inv.conservativeResizeLike(Eigen::VectorXd::Zero(6)); + tmp.conservativeResizeLike(Eigen::VectorXd::Zero(6)); + ESTIMATION_GAIN.conservativeResizeLike(Eigen::VectorXd::Constant(nj, ESTIMATION_GAIN(0))); + dynparam_solver.updateInternalDataStructures(); + jacobian_solver.updateInternalDataStructures(); + fk_pos_solver.updateInternalDataStructures(); +} + +// Calculates robot's initial momentum in the joint space. If this method is not called by the user, zero values will be taken for the initial momentum. +int ChainExternalWrenchEstimator::setInitialMomentum(const JntArray &joint_position, const JntArray &joint_velocity) +{ + // Check sizes first + if (joint_position.rows() != nj || joint_velocity.rows() != nj) + return (error = E_SIZE_MISMATCH); + + // Calculate robot's inertia and momentum in the joint space + if (E_NOERROR != dynparam_solver.JntToMass(joint_position, jnt_mass_matrix)) + return (error = E_DYNPARAMSOLVERMASS_FAILED); + + initial_jnt_momentum.data = jnt_mass_matrix.data * joint_velocity.data; + + // Reset data because of the new momentum offset + SetToZero(estimated_momentum_integral); + SetToZero(filtered_estimated_ext_torque); + + return (error = E_NOERROR); +} + +// Sets singular-value eps parameter for the SVD calculation +void ChainExternalWrenchEstimator::setSVDEps(const double eps_in) +{ + svd_eps = eps_in; +} + +// Sets maximum iteration parameter for the SVD calculation +void ChainExternalWrenchEstimator::setSVDMaxIter(const int maxiter_in) +{ + svd_maxiter = maxiter_in; +} + +// This method calculates the external wrench that is applied on the robot's end-effector. +int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position, const JntArray &joint_velocity, const JntArray &joint_torque, Wrench &external_wrench) +{ + /** + * ========================================================================== + * First-order momentum observer, an implementation based on: + * S. Haddadin, A. De Luca and A. Albu-Schäffer, + * "Robot Collisions: A Survey on Detection, Isolation, and Identification," + * in IEEE Transactions on Robotics, vol. 33(6), pp. 1292-1312, 2017. + * ========================================================================== + */ + + // Check sizes first + if (nj != CHAIN.getNrOfJoints() || ns != CHAIN.getNrOfSegments()) + return (error = E_NOT_UP_TO_DATE); + if (joint_position.rows() != nj || joint_velocity.rows() != nj || joint_torque.rows() != nj) + return (error = E_SIZE_MISMATCH); + + /** + * ================================================================================================================ + * Part I: Estimation of the torques felt in joints as a result of the external wrench being applied on the robot + * ================================================================================================================ + */ + + // Calculate decomposed robot's dynamics + if (E_NOERROR != dynparam_solver.JntToMass(joint_position, jnt_mass_matrix)) + return (error = E_DYNPARAMSOLVERMASS_FAILED); + + if (E_NOERROR != dynparam_solver.JntToCoriolis(joint_position, joint_velocity, coriolis_torque)) + return (error = E_DYNPARAMSOLVERCORIOLIS_FAILED); + + if (E_NOERROR != dynparam_solver.JntToGravity(joint_position, gravity_torque)) + return (error = E_DYNPARAMSOLVERGRAVITY_FAILED); + + // Calculate the change of robot's inertia in the joint space + jnt_mass_matrix_dot.data = (jnt_mass_matrix.data - previous_jnt_mass_matrix.data) / DT_SEC; + + // Save data for the next iteration + previous_jnt_mass_matrix.data = jnt_mass_matrix.data; + + // Calculate total torque exerted on the joint + total_torque.data = joint_torque.data - gravity_torque.data - coriolis_torque.data + jnt_mass_matrix_dot.data * joint_velocity.data; + + // Accumulate main integral + estimated_momentum_integral.data += (total_torque.data + filtered_estimated_ext_torque.data) * DT_SEC; + + // Estimate external joint torque + estimated_ext_torque.data = ESTIMATION_GAIN.asDiagonal() * (jnt_mass_matrix.data * joint_velocity.data - estimated_momentum_integral.data - initial_jnt_momentum.data); + + // First order low-pass filter: filter out the noise from the estimated signal + // This filter can be turned off by setting FILTER_CONST value to 0 + filtered_estimated_ext_torque.data = FILTER_CONST * filtered_estimated_ext_torque.data + (1.0 - FILTER_CONST) * estimated_ext_torque.data; + + /** + * ================================================================================================================== + * Part II: Propagate above-estimated joint torques to Cartesian wrench using a pseudo-inverse of Jacobian-Transpose + * ================================================================================================================== + */ + + // Compute robot's end-effector frame, expressed in the base frame + Frame end_eff_frame; + if (E_NOERROR != fk_pos_solver.JntToCart(joint_position, end_eff_frame)) + return (error = E_FKSOLVERPOS_FAILED); + + // Compute robot's jacobian for the end-effector frame, expressed in the base frame + if (E_NOERROR != jacobian_solver.JntToJac(joint_position, jacobian_end_eff)) + return (error = E_JACSOLVER_FAILED); + + // Transform the jacobian from the base frame to the end-effector frame. + // This part can be commented out if the user wants estimated wrench to be expressed w.r.t. base frame + jacobian_end_eff.changeBase(end_eff_frame.M.Inverse()); // Jacobian is now expressed w.r.t. end-effector frame + + // SVD of "Jac^T" with maximum iterations "maxiter": Jac^T = U * S^-1 * V^T + jacobian_end_eff_transpose = jacobian_end_eff.data.transpose(); + if (E_NOERROR != svd_eigen_HH(jacobian_end_eff_transpose, U, S, V, tmp, svd_maxiter)) + return (error = E_SVD_FAILED); + + // Invert singular values: S^-1 + for (int i = 0; i < S.size(); ++i) + S_inv(i) = (std::fabs(S(i)) < svd_eps) ? 0.0 : 1.0 / S(i); + + // Compose the inverse: (Jac^T)^-1 = V * S^-1 * U^T + jacobian_end_eff_transpose_inv = V * S_inv.asDiagonal() * U.adjoint(); + + // Compute end-effector's Cartesian wrench from the estimated joint torques: (Jac^T)^-1 * ext_tau + Vector6d estimated_wrench = jacobian_end_eff_transpose_inv * filtered_estimated_ext_torque.data; + for (int i = 0; i < 6; i++) + external_wrench(i) = estimated_wrench(i); + + return (error = E_NOERROR); +} + +// Getter for the torques felt in the robot's joints due to the external wrench being applied on the robot +void ChainExternalWrenchEstimator::getEstimatedJntTorque(JntArray &external_joint_torque) +{ + assert(external_joint_torque.rows() == filtered_estimated_ext_torque.rows()); + external_joint_torque = filtered_estimated_ext_torque; +} + +const char* ChainExternalWrenchEstimator::strError(const int error) const +{ + if (E_FKSOLVERPOS_FAILED == error) return "Internally-used Forward Position Kinematics (Recursive) solver failed"; + else if (E_JACSOLVER_FAILED == error) return "Internally-used Jacobian solver failed"; + else if (E_DYNPARAMSOLVERMASS_FAILED == error) return "Internally-used Dynamics Parameters (Mass) solver failed"; + else if (E_DYNPARAMSOLVERCORIOLIS_FAILED == error) return "Internally-used Dynamics Parameters (Coriolis) solver failed"; + else if (E_DYNPARAMSOLVERGRAVITY_FAILED == error) return "Internally-used Dynamics Parameters (Gravity) solver failed"; + else return SolverI::strError(error); +} + +} // namespace diff --git a/orocos_kdl/src/chainexternalwrenchestimator.hpp b/orocos_kdl/src/chainexternalwrenchestimator.hpp new file mode 100644 index 000000000..3147a5df3 --- /dev/null +++ b/orocos_kdl/src/chainexternalwrenchestimator.hpp @@ -0,0 +1,128 @@ +// Copyright (C) 2021 Djordje Vukcevic + +// Version: 1.0 +// Author: Djordje Vukcevic +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_CHAIN_EXTERNAL_WRENCH_ESTIMATOR_HPP +#define KDL_CHAIN_EXTERNAL_WRENCH_ESTIMATOR_HPP + +#include +#include "utilities/svd_eigen_HH.hpp" +#include "chaindynparam.hpp" +#include "chainjnttojacsolver.hpp" +#include "chainfksolverpos_recursive.hpp" +#include + +namespace KDL { + + /** + * \brief First-order momentum observer for the estimation of external wrenches applied on the robot's end-effector. + * + * Implementation based on: + * S. Haddadin, A. De Luca and A. Albu-Schäffer, + * "Robot Collisions: A Survey on Detection, Isolation, and Identification," + * in IEEE Transactions on Robotics, vol. 33(6), pp. 1292-1312, 2017. + * + * Note: This component assumes that the external wrench is applied on the end-effector (last) link of the robot's chain. + */ + class ChainExternalWrenchEstimator : public SolverI + { + typedef Eigen::Matrix Vector6d; + + public: + + static const int E_FKSOLVERPOS_FAILED = -100; //! Internally-used Forward Position Kinematics (Recursive) solver failed + static const int E_JACSOLVER_FAILED = -101; //! Internally-used Jacobian solver failed + static const int E_DYNPARAMSOLVERMASS_FAILED = -102; //! Internally-used Dynamics Parameters (Mass) solver failed + static const int E_DYNPARAMSOLVERCORIOLIS_FAILED = -103; //! Internally-used Dynamics Parameters (Coriolis) solver failed + static const int E_DYNPARAMSOLVERGRAVITY_FAILED = -104; //! Internally-used Dynamics Parameters (Gravity) solver failed + + /** + * Constructor for the estimator, it will allocate all the necessary memory + * \param chain The kinematic chain of the robot, an internal copy will be made. + * \param gravity The gravity-acceleration vector to use during the calculation. + * \param sample_frequency Frequency at which users updates it estimation loop (in Hz). + * \param estimation_gain Parameter used to control the estimator's convergence + * \param filter_constant Parameter defining how much the estimated signal should be filtered by the low-pass filter. + * This input value should be between 0 and 1. Higher the number means more noise needs to be filtered-out. + * The filter can be turned off by setting this value to 0. + * \param eps If a SVD-singular value is below this value, its inverse is set to zero. Default: 0.00001 + * \param maxiter Maximum iterations for the SVD computations. Default: 150. + */ + ChainExternalWrenchEstimator(const Chain &chain, const Vector &gravity, const double sample_frequency, const double estimation_gain, const double filter_constant, const double eps = 0.00001, const int maxiter = 150); + ~ChainExternalWrenchEstimator(){}; + + /** + * Calculates robot's initial momentum in the joint space. + * Basically, sets the offset for future estimation (momentum calculation). + * If this method is not called by the user, zero values will be taken for the initial momentum. + */ + int setInitialMomentum(const JntArray &joint_position, const JntArray &joint_velocity); + + // Sets singular-value eps parameter for the SVD calculation + void setSVDEps(const double eps_in); + + // Sets maximum iteration parameter for the SVD calculation + void setSVDMaxIter(const int maxiter_in); + + /** + * This method calculates the external wrench that is applied on the robot's end-effector. + * Input parameters: + * \param joint_position The current (measured) joint positions. + * \param joint_velocity The current (measured) joint velocities. + * \param joint_torque The joint space torques. + * Depending on the user's choice, this array can represent commanded or measured joint torques. + * A particular choice depends on the available sensors in robot's joint. + * For more details see the above-referenced article. + * + * Output parameters: + * \param external_wrench The estimated external wrench applied on the robot's end-effector. + * The wrench will be expressed w.r.t. end-effector's frame. + * + * @return error/success code + */ + int JntToExtWrench(const JntArray &joint_position, const JntArray &joint_velocity, const JntArray &joint_torque, Wrench &external_wrench); + + // Returns the torques felt in the robot's joints as a result of the external wrench being applied on the robot. + void getEstimatedJntTorque(JntArray &external_joint_torque); + + /// @copydoc KDL::SolverI::updateInternalDataStructures() + virtual void updateInternalDataStructures(); + + /// @copydoc KDL::SolverI::strError() + virtual const char* strError(const int error) const; + + private: + const Chain &CHAIN; + const double DT_SEC, FILTER_CONST; + double svd_eps; + int svd_maxiter; + unsigned int nj, ns; + JntSpaceInertiaMatrix jnt_mass_matrix, previous_jnt_mass_matrix, jnt_mass_matrix_dot; + JntArray initial_jnt_momentum, estimated_momentum_integral, filtered_estimated_ext_torque, + gravity_torque, coriolis_torque, total_torque, estimated_ext_torque; + Jacobian jacobian_end_eff; + Eigen::MatrixXd jacobian_end_eff_transpose, jacobian_end_eff_transpose_inv, U, V; + Eigen::VectorXd S, S_inv, tmp, ESTIMATION_GAIN; + ChainDynParam dynparam_solver; + ChainJntToJacSolver jacobian_solver; + ChainFkSolverPos_recursive fk_pos_solver; + }; +} + +#endif diff --git a/orocos_kdl/src/chainfdsolver.hpp b/orocos_kdl/src/chainfdsolver.hpp new file mode 100644 index 000000000..a960ccd7f --- /dev/null +++ b/orocos_kdl/src/chainfdsolver.hpp @@ -0,0 +1,62 @@ +// Copyright (C) 2018 Ruben Smits + +// Version: 1.0 +// Author: Ruben Smits +// Author: Craig Carignan +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_CHAIN_FDSOLVER_HPP +#define KDL_CHAIN_FDSOLVER_HPP + +#include "chain.hpp" +#include "frames.hpp" +#include "jntarray.hpp" +#include "solveri.hpp" + +namespace KDL +{ + + typedef std::vector Wrenches; + + /** + * \brief This abstract class encapsulates the inverse + * dynamics solver for a KDL::Chain. + * + */ + class ChainFdSolver : public KDL::SolverI + { + public: + /** + * Calculate forward dynamics from joint positions, joint velocities, joint torques/forces, + * and externally applied forces/torques to joint accelerations. + * + * @param q input joint positions + * @param q_dot input joint velocities + * @param torque input joint torques + * @param f_ext external forces + * + * @param q_dotdot output joint accelerations + * + * @return if < 0 something went wrong + */ + virtual int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches& f_ext,JntArray &q_dotdot)=0; + }; + +} + +#endif diff --git a/orocos_kdl/src/chainfdsolver_recursive_newton_euler.cpp b/orocos_kdl/src/chainfdsolver_recursive_newton_euler.cpp new file mode 100644 index 000000000..6e01c49a4 --- /dev/null +++ b/orocos_kdl/src/chainfdsolver_recursive_newton_euler.cpp @@ -0,0 +1,138 @@ +// Copyright (C) 2018 Craig Carignan + +// Version: 1.0 +// Author: Craig Carignan +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "chainfdsolver_recursive_newton_euler.hpp" +#include "utilities/ldl_solver_eigen.hpp" +#include "frames_io.hpp" +#include "kinfam_io.hpp" + +namespace KDL{ + + ChainFdSolver_RNE::ChainFdSolver_RNE(const Chain& _chain, Vector _grav): + chain(_chain), + DynSolver(chain, _grav), + IdSolver(chain, _grav), + nj(chain.getNrOfJoints()), + ns(chain.getNrOfSegments()), + H(nj), + Tzeroacc(nj), + H_eig(nj,nj), + Tzeroacc_eig(nj), + L_eig(nj,nj), + D_eig(nj), + r_eig(nj), + acc_eig(nj) + { + } + + void ChainFdSolver_RNE::updateInternalDataStructures() { + nj = chain.getNrOfJoints(); + ns = chain.getNrOfSegments(); + } + + int ChainFdSolver_RNE::CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches& f_ext, JntArray &q_dotdot) + { + if(nj != chain.getNrOfJoints() || ns != chain.getNrOfSegments()) + return (error = E_NOT_UP_TO_DATE); + + //Check sizes of function parameters + if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj || f_ext.size()!=ns) + return (error = E_SIZE_MISMATCH); + + // Inverse Dynamics: + // T = H * qdd + Tcor + Tgrav - J^T * Fext + // Forward Dynamics: + // 1. Call ChainDynParam->JntToMass to get H + // 2. Call ChainIdSolver_RNE->CartToJnt with qdd=0 to get Tcor+Tgrav-J^T*Fext + // 3. Calculate qdd = H^-1 * T where T are applied joint torques minus non-inertial internal torques + + // Calculate Joint Space Inertia Matrix + error = DynSolver.JntToMass(q, H); + if (error < 0) + return (error); + + // Calculate non-inertial internal torques by inputting zero joint acceleration to ID + for(unsigned int i=0;i + +// Version: 1.0 +// Author: Craig Carignan +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_CHAIN_FDSOLVER_RECURSIVE_NEWTON_EULER_HPP +#define KDL_CHAIN_FDSOLVER_RECURSIVE_NEWTON_EULER_HPP + +#include "chainfdsolver.hpp" +#include "chainidsolver_recursive_newton_euler.hpp" +#include "chaindynparam.hpp" + +namespace KDL{ + /** + * \brief Recursive newton euler forward dynamics solver + * + * The algorithm implementation is based on the book "Rigid Body + * Dynamics Algorithms" of Roy Featherstone, 2008 + * (ISBN:978-0-387-74314-1) See Chapter 6 for basic algorithm. + * + * It calculates the accelerations for the joints (qdotdot), given the + * position and velocity of the joints (q,qdot,qdotdot), external forces + * on the segments (expressed in the segments reference frame), + * and the dynamical parameters of the segments. + */ + class ChainFdSolver_RNE : public ChainFdSolver{ + public: + /** + * Constructor for the solver, it will allocate all the necessary memory + * \param chain The kinematic chain to calculate the forward dynamics for, an internal copy will be made. + * \param grav The gravity vector to use during the calculation. + */ + ChainFdSolver_RNE(const Chain& chain, Vector grav); + ~ChainFdSolver_RNE(){}; + + /** + * Function to calculate from Cartesian forces to joint torques. + * Input parameters; + * \param q The current joint positions + * \param q_dot The current joint velocities + * \param torques The current joint torques (applied by controller) + * \param f_ext The external forces (no gravity) on the segments + * Output parameters: + * \param q_dotdot The resulting joint accelerations + */ + int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches& f_ext, JntArray &q_dotdot); + + /// @copydoc KDL::SolverI::updateInternalDataStructures + virtual void updateInternalDataStructures(); + + /** + * Function to integrate the joint accelerations resulting from the forward dynamics solver. + * Input parameters; + * \param nj The number of joints + * \param t The current time + * \param dt The integration period + * \param q The current joint positions + * \param q_dot The current joint velocities + * \param torques The current joint torques (applied by controller) + * \param f_ext The external forces (no gravity) on the segments + * \param fdsolver The forward dynamics solver + * Output parameters: + * \param t The updated time + * \param q The updated joint positions + * \param q_dot The updated joint velocities + * \param q_dotdot The current joint accelerations + * \param dq The joint position increment + * \param dq_dot The joint velocity increment + * Temporary parameters: + * \param qtemp Intermediate joint positions + * \param qdtemp Intermediate joint velocities + */ + void RK4Integrator(unsigned int& nj, const double& t, double& dt, KDL::JntArray& q, KDL::JntArray& q_dot, + KDL::JntArray& torques, KDL::Wrenches& f_ext, KDL::ChainFdSolver_RNE& fdsolver, + KDL::JntArray& q_dotdot, KDL::JntArray& dq, KDL::JntArray& dq_dot, + KDL::JntArray& q_temp, KDL::JntArray& q_dot_temp); + + private: + const Chain& chain; + ChainDynParam DynSolver; + ChainIdSolver_RNE IdSolver; + unsigned int nj; + unsigned int ns; + JntSpaceInertiaMatrix H; + JntArray Tzeroacc; + Eigen::MatrixXd H_eig; + Eigen::VectorXd Tzeroacc_eig; + Eigen::MatrixXd L_eig; + Eigen::VectorXd D_eig; + Eigen::VectorXd r_eig; + Eigen::VectorXd acc_eig; + }; +} + +#endif diff --git a/orocos_kdl/src/chainfksolver.hpp b/orocos_kdl/src/chainfksolver.hpp index f82c20d13..226cd4f67 100644 --- a/orocos_kdl/src/chainfksolver.hpp +++ b/orocos_kdl/src/chainfksolver.hpp @@ -32,8 +32,8 @@ namespace KDL { /** - * \brief This abstract class encapsulates a - * solver for the forward position kinematics for a KDL::Chain. + * \brief This abstract class encapsulates a + * solver for the forward position kinematics for a KDL::Chain. * * @ingroup KinematicFamily */ @@ -104,11 +104,10 @@ namespace KDL { * * @ingroup KinematicFamily */ - class ChainFkSolverAcc : public KDL::SolverI { public: /** - * Calculate forward position, velocity and accelaration + * Calculate forward position, velocity and acceleration * kinematics, from joint coordinates to cartesian coordinates * * @param q_in input joint coordinates (position, velocity and @@ -120,12 +119,12 @@ namespace KDL { */ virtual int JntToCart(const JntArrayAcc& q_in, FrameAcc& out,int segmentNr=-1)=0; /** - * Calculate forward position, velocity and accelaration + * Calculate forward position, velocity and acceleration * kinematics, from joint coordinates to cartesian coordinates * * @param q_in input joint coordinates (position, velocity and * acceleration - @param out output cartesian coordinates (position, velocity + * @param out output cartesian coordinates (position, velocity * and acceleration for all segments * * @return if < 0 something went wrong diff --git a/orocos_kdl/src/chainfksolverpos_recursive.cpp b/orocos_kdl/src/chainfksolverpos_recursive.cpp index 99d7d4793..487569c85 100644 --- a/orocos_kdl/src/chainfksolverpos_recursive.cpp +++ b/orocos_kdl/src/chainfksolverpos_recursive.cpp @@ -1,9 +1,10 @@ // Copyright (C) 2007 Francois Cauwe -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2007 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Francois Cauwe +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -46,7 +47,7 @@ namespace KDL { else{ int j=0; for(unsigned int i=0;i -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2007 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Francois Cauwe +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -51,7 +52,7 @@ namespace KDL int j=0; for (unsigned int i=0;i + +// Version: 1.0 +// Author: Ruben Smits +// Author: Herman Bruyninckx +// Author: Azamat Shakhimardanov +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "chainhdsolver_vereshchagin.hpp" +#include "frames_io.hpp" +#include "utilities/svd_eigen_HH.hpp" + +namespace KDL +{ + +ChainHdSolver_Vereshchagin::ChainHdSolver_Vereshchagin(const Chain& chain_, const Twist &root_acc, const unsigned int nc_) : + chain(chain_), nj(chain.getNrOfJoints()), ns(chain.getNrOfSegments()), nc(nc_), + results(ns + 1, segment_info(nc)) +{ + acc_root = root_acc; + + //Provide the necessary memory for computing the inverse of M0 + nu_sum.resize(nc); + M_0_inverse.resize(nc, nc); + Um = Eigen::MatrixXd::Identity(nc, nc); + Vm = Eigen::MatrixXd::Identity(nc, nc); + Sm = Eigen::VectorXd::Ones(nc); + tmpm = Eigen::VectorXd::Ones(nc); + + // Provide the necessary memory for storing the total torque acting on each joint + total_torques = Eigen::VectorXd::Zero(nj); +} + +void ChainHdSolver_Vereshchagin::updateInternalDataStructures() { + ns = chain.getNrOfSegments(); + nj = chain.getNrOfJoints(); + total_torques = Eigen::VectorXd::Zero(nj); + results.resize(ns+1,segment_info(nc)); +} + +int ChainHdSolver_Vereshchagin::CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, const JntArray &ff_torques, JntArray &constraint_torques) +{ + //Check sizes always + nj = chain.getNrOfJoints(); + if(ns != chain.getNrOfSegments()) + return (error = E_NOT_UP_TO_DATE); + if (q.rows() != nj || q_dot.rows() != nj || q_dotdot.rows() != nj || ff_torques.rows() != nj || constraint_torques.rows() != nj || f_ext.size() != ns) + return (error = E_SIZE_MISMATCH); + if (alfa.columns() != nc || beta.rows() != nc) + return (error = E_SIZE_MISMATCH); + //do an upward recursion for position, velocities and rigid-body bias forces + this->initial_upwards_sweep(q, q_dot, q_dotdot, f_ext); + //do an inward recursion for inertia, articulated bias forces and constraints + this->downwards_sweep(alfa, ff_torques); + //Solve for the constraint forces + this->constraint_calculation(beta); + //do an upward recursion to propagate the result and compute final output + this->final_upwards_sweep(q_dotdot, constraint_torques); + return (error = E_NOERROR); +} + +void ChainHdSolver_Vereshchagin::initial_upwards_sweep(const JntArray &q, const JntArray &qdot, const JntArray &qdotdot, const Wrenches& f_ext) +{ + //if (q.rows() != nj || qdot.rows() != nj || qdotdot.rows() != nj || f_ext.size() != ns) + // return -1; + + unsigned int j = 0; + F_total = Frame::Identity(); + for (unsigned int i = 0; i < ns; i++) + { + //Express everything in the segments reference frame (body coordinates) + //which is at the segments tip, i.e. where the next joint is attached. + + //Calculate segment properties: X,S,vj,cj + const Segment& segment = chain.getSegment(i); + segment_info& s = results[i + 1]; + //The pose between the joint root and the segment tip (tip expressed in joint root coordinates) + s.F = segment.pose(q(j)); //X pose of each link in link coord system + + F_total = F_total * s.F; //X pose of the each link in root coord system + s.F_base = F_total; //X pose of the each link in root coord system for getter functions + + //The velocity due to the joint motion of the segment expressed in the segments reference frame (tip) + Twist vj = s.F.M.Inverse(segment.twist(q(j), qdot(j))); //XDot of each link + //Twist aj = s.F.M.Inverse(segment.twist(q(j), qdotdot(j))); //XDotDot of each link + + //The unit velocity due to the joint motion of the segment expressed in the segments reference frame (tip) + s.Z = s.F.M.Inverse(segment.twist(q(j), 1.0)); + //Put Z in the joint root reference frame: + s.Z = s.F * s.Z; + + //The total velocity of the segment expressed in the segments reference frame (tip) + if (i != 0) + { + s.v = s.F.Inverse(results[i].v) + vj; // recursive velocity of each link in segment frame + //s.A=s.F.Inverse(results[i].A)+aj; + s.A = s.F.M.Inverse(results[i].A); + } + else + { + s.v = vj; + s.A = s.F.M.Inverse(acc_root); + } + //c[i] = cj + v[i]xvj (remark: cj=0, since our S is not time dependent in local coordinates) + //The velocity product acceleration + //std::cout << i << " Initial upward" << s.v << std::endl; + s.C = s.v*vj; //This is a cross product: cartesian space BIAS acceleration in local link coord. + //Put C in the joint root reference frame + s.C = s.F * s.C; //+F_total.M.Inverse(acc_root)); + //The rigid body inertia of the segment, expressed in the segments reference frame (tip) + s.H = segment.getInertia(); + + //wrench of the rigid body bias forces and the external forces on the segment (in body coordinates, tip) + //external forces are taken into account through s.U. + Wrench FextLocal = F_total.M.Inverse() * f_ext[i]; + s.U = s.v * (s.H * s.v) - FextLocal; //f_ext[i]; + if (segment.getJoint().getType() != Joint::Fixed) + j++; + } + +} + +void ChainHdSolver_Vereshchagin::downwards_sweep(const Jacobian& alfa, const JntArray &ff_torques) +{ + int j = nj - 1; + for (int i = ns; i >= 0; i--) + { + //Get a handle for the segment we are working on. + segment_info& s = results[i]; + //For segment N, + //tilde is in the segment refframe (tip, not joint root) + //without tilde is at the joint root (the childs tip!!!) + //P_tilde is the articulated body inertia + //R_tilde is the sum of external and coriolis/centrifugal forces + //M is the (unit) acceleration energy already generated at link i + //G is the (unit) magnitude of the constraint forces at link i + //E are the (unit) constraint forces due to the constraints + if (i == (int)ns) + { + s.P_tilde = s.H; + s.R_tilde = s.U; + s.M.setZero(); + s.G.setZero(); + //changeBase(alfa_N,F_total.M.Inverse(),alfa_N2); + for (unsigned int r = 0; r < 3; r++) + for (unsigned int c = 0; c < nc; c++) + { + //copy alfa constrain force matrix in E~ + s.E_tilde(r, c) = alfa(r + 3, c); + s.E_tilde(r + 3, c) = alfa(r, c); + } + //Change the reference frame of alfa to the segmentN tip frame + //F_Total holds end effector frame, if done per segment bases then constraints could be extended to all segments + Rotation base_to_end = F_total.M.Inverse(); + for (unsigned int c = 0; c < nc; c++) + { + Wrench col(Vector(s.E_tilde(3, c), s.E_tilde(4, c), s.E_tilde(5, c)), + Vector(s.E_tilde(0, c), s.E_tilde(1, c), s.E_tilde(2, c))); + col = base_to_end*col; + s.E_tilde.col(c) << Eigen::Vector3d::Map(col.torque.data), Eigen::Vector3d::Map(col.force.data); + } + } + else + { + //For all others: + //Everything should expressed in the body coordinates of segment i + segment_info& child = results[i + 1]; + //Copy PZ into a vector so we can do matrix manipulations, put torques above forces + Vector6d vPZ; + vPZ << Eigen::Vector3d::Map(child.PZ.torque.data), Eigen::Vector3d::Map(child.PZ.force.data); + Matrix6d PZDPZt; + PZDPZt.noalias() = vPZ * vPZ.transpose(); + PZDPZt /= child.D; + + //equation a) (see Vereshchagin89) PZDPZt=[I,H;H',M] + //Azamat:articulated body inertia as in Featherstone (7.19) + s.P_tilde = s.H + child.P - ArticulatedBodyInertia(PZDPZt.bottomRightCorner<3,3>(), PZDPZt.topRightCorner<3,3>(), PZDPZt.topLeftCorner<3,3>()); + //equation b) (see Vereshchagin89) + //Azamat: bias force as in Featherstone (7.20) + s.R_tilde = s.U + child.R + child.PC + (child.PZ / child.D) * child.u; + //equation c) (see Vereshchagin89) + s.E_tilde = child.E; + + //Azamat: equation (c) right side term + s.E_tilde.noalias() -= (vPZ * child.EZ.transpose()) / child.D; + + //equation d) (see Vereshchagin89) + s.M = child.M; + //Azamat: equation (d) right side term + s.M.noalias() -= (child.EZ * child.EZ.transpose()) / child.D; + + //equation e) (see Vereshchagin89) + s.G = child.G; + Twist CiZDu = child.C + (child.Z / child.D) * child.u; + Vector6d vCiZDu; + vCiZDu << Eigen::Vector3d::Map(CiZDu.rot.data), Eigen::Vector3d::Map(CiZDu.vel.data); + s.G.noalias() += child.E.transpose() * vCiZDu; + } + if (i != 0) + { + //Transform all results to joint root coordinates of segment i (== body coordinates segment i-1) + //equation a) + s.P = s.F * s.P_tilde; + //equation b) + s.R = s.F * s.R_tilde; + //equation c), in matrix: torques above forces, so switch and switch back + for (unsigned int c = 0; c < nc; c++) + { + Wrench col(Vector(s.E_tilde(3, c), s.E_tilde(4, c), s.E_tilde(5, c)), + Vector(s.E_tilde(0, c), s.E_tilde(1, c), s.E_tilde(2, c))); + col = s.F*col; + s.E.col(c) << Eigen::Vector3d::Map(col.torque.data), Eigen::Vector3d::Map(col.force.data); + } + + //needed for next recursion + s.PZ = s.P * s.Z; + + /** + * Additionally adding joint inertia to s.D, see: + * - equation a) in Vereshchagin89 + * - equation 9.28, page 188, Featherstone book 2008 + */ + if (chain.getSegment(i - 1).getJoint().getType() != Joint::Fixed) + s.D = chain.getSegment(i - 1).getJoint().getInertia() + dot(s.Z, s.PZ); + else + s.D = dot(s.Z, s.PZ); + + s.PC = s.P * s.C; + + //u=(Q-Z(R+PC)=sum of external forces along the joint axes, + //R are the forces coming from the children, + //Q is taken zero (do we need to take the previous calculated torques? + + //projection of coriolis and centrepital forces into joint subspace (0 0 Z) + s.totalBias = -dot(s.Z, s.R + s.PC); + s.u = ff_torques(j) + s.totalBias; + + //Matrix form of Z, put rotations above translations + Vector6d vZ; + vZ << Eigen::Vector3d::Map(s.Z.rot.data), Eigen::Vector3d::Map(s.Z.vel.data); + s.EZ.noalias() = s.E.transpose() * vZ; + + if (chain.getSegment(i - 1).getJoint().getType() != Joint::Fixed) + j--; + } + } +} + +void ChainHdSolver_Vereshchagin::constraint_calculation(const JntArray& beta) +{ + //equation f) nu = M_0_inverse*(beta_N - E0_tilde`*acc0 - G0) + //M_0_inverse, always nc*nc symmetric matrix + //std::cout<<"M0: "< axis torque/force + constraint_torques(j) = -dot(s.Z, constraint_force); + //The result should be the torque originating from the end-effector constraints + + // Total torque on the joint resulting from the parent forces, constraint forces and nullspace forces. + total_torques(j) = s.u + parent_forceProjection + constraint_torques(j); + // q_dotdot(j) is also equal to: total_torques(j) / s.D + + s.constAccComp = constraint_torques(j) / s.D; + s.nullspaceAccComp = s.u / s.D; + + // Total joint space acceleration resulting from accelerations of parent joints, constraint forces and nullspace forces. + q_dotdot(j) = (s.nullspaceAccComp + parentAccComp + s.constAccComp); + // Returns segment's spatial acceleration in link distal-tip coordinates. For use needs to be transformed + s.acc = s.F.Inverse(a_p + s.Z * q_dotdot(j) + s.C); + if (chain.getSegment(i - 1).getJoint().getType() != Joint::Fixed) + j++; + } +} + +// Returns Cartesian acceleration of links in robot base coordinates +void ChainHdSolver_Vereshchagin::getTransformedLinkAcceleration(Twists& x_dotdot) +{ + assert(x_dotdot.size() == ns + 1); + x_dotdot[0] = acc_root; + for (unsigned int i = 1; i < ns + 1; i++) + x_dotdot[i] = results[i].F_base.M * results[i].acc; +} + +// Returns total torque acting on each joint (constraints + nature + external forces) +void ChainHdSolver_Vereshchagin::getTotalTorque(JntArray &total_tau) +{ + assert(total_tau.data.size() == total_torques.size()); + total_tau.data = total_torques; +} + +// Returns magnitude of the constraint forces acting on the end-effector: Lagrange Multiplier +void ChainHdSolver_Vereshchagin::getContraintForceMagnitude(Eigen::VectorXd &nu_) +{ + assert(nu_.size() == nu.size()); + nu_ = nu; +} + +/* +void ChainHdSolver_Vereshchagin::getLinkCartesianPose(Frames& x_base) +{ + for (int i = 0; i < ns; i++) + { + x_base[i] = results[i + 1].F_base; + } + return; +} + +void ChainHdSolver_Vereshchagin::getLinkCartesianVelocity(Twists& xDot_base) +{ + + for (int i = 0; i < ns; i++) + { + xDot_base[i] = results[i + 1].F_base.M * results[i + 1].v; + } + return; +} + +void ChainHdSolver_Vereshchagin::getLinkCartesianAcceleration(Twists& xDotDot_base) +{ + + for (int i = 0; i < ns; i++) + { + xDotDot_base[i] = results[i + 1].F_base.M * results[i + 1].acc; + //std::cout << "XDotDot_base[i] " << xDotDot_base[i] << std::endl; + } + return; +} + +void ChainHdSolver_Vereshchagin::getLinkPose(Frames& x_local) +{ + for (int i = 0; i < ns; i++) + { + x_local[i] = results[i + 1].F; + } + return; +} + +void ChainHdSolver_Vereshchagin::getLinkVelocity(Twists& xDot_local) +{ + for (int i = 0; i < ns; i++) + { + xDot_local[i] = results[i + 1].v; + } + return; + +} + +void ChainHdSolver_Vereshchagin::getLinkAcceleration(Twists& xDotdot_local) +{ + for (int i = 0; i < ns; i++) + { + xDotdot_local[i] = results[i + 1].acc; + } + return; + +} + +void ChainHdSolver_Vereshchagin::getJointBiasAcceleration(JntArray& bias_q_dotdot) +{ + for (int i = 0; i < ns; i++) + { + //this is only force + double tmp = results[i + 1].totalBias; + //this is acceleration + bias_q_dotdot(i) = tmp / results[i + 1].D; + + //s.totalBias = - dot(s.Z, s.R + s.PC); + //std::cout << "totalBiasAccComponent" << i << ": " << bias_q_dotdot(i) << std::endl; + //bias_q_dotdot(i) = s.totalBias/s.D + + } + return; + +} + +void ChainHdSolver_Vereshchagin::getJointConstraintAcceleration(JntArray& constraint_q_dotdot) +{ + for (int i = 0; i < ns; i++) + { + constraint_q_dotdot(i) = results[i + 1].constAccComp; + //double tmp = results[i + 1].u; + //s.u = torques(j) + s.totalBias; + // std::cout << "s.constraintAccComp" << i << ": " << results[i+1].constAccComp << std::endl; + //nullspace_q_dotdot(i) = s.u/s.D + + } + return; + + +} + +//Check the name it does not seem to be appropriate + +void ChainHdSolver_Vereshchagin::getJointNullSpaceAcceleration(JntArray& nullspace_q_dotdot) +{ + for (int i = 0; i < ns; i++) + { + nullspace_q_dotdot(i) = results[i + 1].nullspaceAccComp; + //double tmp = results[i + 1].u; + //s.u = torques(j) + s.totalBias; + //std::cout << "s.nullSpaceAccComp" << i << ": " << results[i+1].nullspaceAccComp << std::endl; + //nullspace_q_dotdot(i) = s.u/s.D + + } + return; + + +} + +//This is not only a bias force energy but also includes generalized forces +//change type of parameter G +//this method should return array of G's + +void ChainHdSolver_Vereshchagin::getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G) +{ + for (int i = 0; i < ns; i++) + { + G = results[i + 1].G; + //double tmp = results[i + 1].u; + //s.u = torques(j) + s.totalBias; + //std::cout << "s.G " << i << ": " << results[i+1].G << std::endl; + //nullspace_q_dotdot(i) = s.u/s.D + + } + return; + +} + +//this method should return array of R's + +void ChainHdSolver_Vereshchagin::getLinkBiasForceMatrix(Wrenches& R_tilde) +{ + for (int i = 0; i < ns; i++) + { + R_tilde[i] = results[i + 1].R_tilde; + //Azamat: bias force as in Featherstone (7.20) + //s.R_tilde = s.U + child.R + child.PC + child.PZ / child.D * child.u; + std::cout << "s.R_tilde " << i << ": " << results[i + 1].R_tilde << std::endl; + } + return; +} + +*/ + +}//namespace diff --git a/orocos_kdl/src/chainhdsolver_vereshchagin.hpp b/orocos_kdl/src/chainhdsolver_vereshchagin.hpp new file mode 100644 index 000000000..86be6471a --- /dev/null +++ b/orocos_kdl/src/chainhdsolver_vereshchagin.hpp @@ -0,0 +1,535 @@ +// Copyright (C) 2009 Ruben Smits + +// Version: 1.0 +// Author: Ruben Smits +// Author: Herman Bruyninckx +// Author: Azamat Shakhimardanov +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP +#define KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP + +#include "chainidsolver.hpp" +#include "frames.hpp" +#include "articulatedbodyinertia.hpp" + +#include + +namespace KDL +{ +/** + * \brief **Abstract**: Acceleration constrained hybrid dynamics calculations for a chain, based on Vereshchagin 1989. + * This class creates an instance of the hybrid dynamics solver. The solver analytically calculates the joint space + * constraint torques and acceleration in a chain when a constraint force(s) is applied to the chain's end-effector + * (task space / cartesian space). In the robotics literature, this algorithm is also known under the following names: + * Acceleration Constrained Hybrid Dynamics (ACHD) and Popov-Vereshchagin solver. + * + * ## INTRODUCTION + * + * In 1970', researchers [1], [2] have developed a hybrid dynamics algorithm for evaluating robot behavior + * based on the input specification that is defined by the Cartesian acceleration constraints, + * feed-forward joint torques and external Cartesian wrenches. The solver is derived from a well-known + * principle of mechanics - **Gauss' principle of least constraint** [6] and provides an analytical (closed-form) + * solution to the hybrid dynamics problem with linear-time, **O(n)** complexity [3]. + * + * In general, the Gauss' principle states that the true motion (acceleration) of a system/body is defined + * by the minimum of a quadratic function that is subject to linear geometric motion constraints [7], [6]. + * The result of this Gauss function represents the **acceleration energy** of a body, which is defined by + * the product of its mass and the squared distance between its allowed (constrained) acceleration and its free + * (unconstrained) acceleration [10], [11]. In the case of originally derived Popov-Vereshchagin algorithm [1], + * geometric motion constraints are Cartesian acceleration constraints imposed on the robot's end-effector. + * This domain-specific solver minimizes the acceleration energy by performing computational (outward and inward) + * sweeps along the robot's kinematic chain [3]. Furthermore, by computing the minimum of Gauss function, + * the Popov-Vereshchagin solver resolves the kinematic redundancy of the robot, when a partial motion (task) + * specification is provided [2]. A necessary condition that enables this type of closed-form algorithm + * (an analytical solution to the above-described optimization problem) defines that the robot’s kinematic + * chain does not consist of closed loops, i.e. the robot’s kinematic chain must be constructed in a serial + * or tree structure [11]. However, it is always possible to cut these loops and introduce explicit constrains. + * + * For evaluating robot dynamics, i.e. resolving its constrained motion, the Popov-Vereshchagin solver is + * performing three computational sweeps (recursions), along the kinematic chain [3]. More specifically, + * two sweeps in **outward** and one sweep in **inward** direction. In the case of robot dynamics algorithms, + * the outward sweep refers to a recursion that is covering a kinematic chain from proximal to distal segments, + * while the inward sweep is covering a kinematic chain from distal to proximal segments [5]. Additionally, + * after completing the recursion in the second sweep and before starting the recursion in the last sweep, + * the solver is computing magnitudes of constraint forces, i.e. the Langrage multiplier (noted as **nu** in + * the KDL's solver implementation and original solver's publication[2]). More specifically, this operation is + * performed when the algorithm reaches segment (link) **{0}**, namely the base segment. In this formulation + * of the solver, the gravity effects are taken into account by setting the base-link's acceleration equal + * to gravitational acceleration [3]. + * + * For more detailed description of the algorithm and its representation, the reader can refer to [3], [5], [11]. + * + * ## INTERFACES + * + * ### Solver's Input + * + * For computing solutions to the constrained hybrid dynamics problem, this original derivation of + * the Popov-Vereshchagin solver [3] takes into account the following inputs: + * + * * Robot's **model** defined by: kinematic parameters of the chain, segments' mass and rigid-body + * inertia, and effective inertia of each joint rotor -> **chain** parameter in solver's constructor + * + * * **Root** acceleration of the robot's base segment (usually gravitational) -> **root_acc** parameter + * in solver's constructor + * + * * Current joint configuration (angles) -> **q** parameter in the **CartToJnt** function + * + * * Current joint velocities -> **q_dot** parameter in the **CartToJnt** function + * + * * Motion drivers: + * * - **Cartesian Acceleration Constraints** imposed on the end-effector segment -> **alpha** and + * **beta** parameters in the **CartToJnt** function + * * - **Cartesian External Wrench** acting on each segment -> **f_ext** parameter in the **CartToJnt** function + * * - **Feed-Forward Torque** acting on each joint -> **ff_torques** parameter in the **CartToJnt** function + * + * The following outlines the above-listed task interfaces in more detail. + * + * #### Cartesian Acceleration Constraints: alpha & beta + * + * This first type of motion driver can be used for specifying **physical** constraints such as contacts with environment [3], + * or **artificial** (i.e. task-imposed) constraints defined by the operational space task definition for the end-effector + * (tool-tip) segment. **Note**: the Vereshchagin solver expects that the input Cartesian Acceleration Constraints, i.e. + * unit constraint forces in alpha parameters, are expressed w.r.t. robot's base frame. However, the acceleration energy + * setpoints, i.e. beta parameters, are expressed w.r.t. above-defined unit constraint forces. More specifically, each DOF + * (element) in beta parameter corresponds to its respective DOF (column) of the unit constraint force matrix (alpha) [11]. + * + * To use this interface, a user should define **i)** the active constraint directions via **alpha** parameter, which is + * a **6 x m** matrix of spatial unit constraint forces, and **ii)** acceleration energy setpoints via **beta**, which is + * a **m x 1** vector. Here, the number of constraints **m**, or in another words number of spatial unit constraint forces + * is not required to always be equal to **6**, which means that a human programmer can leave some of the degrees of freedom + * unspecified [2] for this motion driver, and still produce valid joint control commands [5]. For example, if we want to + * constrain the motion of the end-effector segment in only one direction, namely linear **x**-direction, we can define + * the constraint as [3]: + * + * **alpha** = + * | | + * | --| + * | 1 | + * | 0 | + * | 0 | + * | 0 | + * | 0 | + * | 0 | + * + * **beta** = | 0 | + * + * Note that here, the first three rows of matrix **alpha** represent linear elements and the last three rows represent + * angular elements, of the spatial unit force defined in Plücker coordinates [4]. By giving zero value to acceleration + * energy setpoint (**beta**), we are defining that the end-effector is not allowed to have linear acceleration in **x** + * direction. Or in other words, we are restricting the robot from producing any acceleration energy in that specified direction. + * + * Another example includes the specification of constraints in **5 DOFs**. We can constrain the motion of robot's end-effector + * such that it is only allowed to **freely** move in the linear **z**-direction, without performing linear motions in **x** + * and **y** and angular motions in **x**, **y** and **z** directions: + * + * **alpha** = + * | | | | | | + * | --| --| --| --| --| + * | 1 | 0 | 0 | 0 | 0 | + * | 0 | 1 | 0 | 0 | 0 | + * | 0 | 0 | 0 | 0 | 0 | + * | 0 | 0 | 1 | 0 | 0 | + * | 0 | 0 | 0 | 1 | 0 | + * | 0 | 0 | 0 | 0 | 1 | + * + * **beta** = + * | | + * | --| + * | 0 | + * | 0 | + * | 0 | + * | 0 | + * | 0 | + * + * For both above-described task examples, the Acceleration Constrained Hybrid Dynamics (ACHD) solver will compute + * valid control (constraint) joint torques, even though some of the Cartesian DOFs are left unspecified + * (e.g. in the case of the second example that would be end-effector's **z**-direction). More specifically: + * ``Underconstrained motion specifications are naturally resolved using Gauss' principle of least constraint`` [5]. + * This means that in those directions in which the robot is not constrained by the task definition, its motions will + * be controlled by the nature. For instance, in the second example, natural resolution of the robot motion would define + * that the end-effector "**falls**" in the linear **z** direction due to effects of gravity, with the assumption that + * gravity forces are acting along $z$-direction. + * + * Moreover, the motion specification in the second example is equivalent to: + * + * **alpha** = + * | | | | | | | + * | --| --| --| --| --| --| + * | 1 | 0 | 0 | 0 | 0 | 0 | + * | 0 | 1 | 0 | 0 | 0 | 0 | + * | 0 | 0 | 0 | 0 | 0 | 0 | + * | 0 | 0 | 0 | 1 | 0 | 0 | + * | 0 | 0 | 0 | 0 | 1 | 0 | + * | 0 | 0 | 0 | 0 | 0 | 1 | + * (note that elements in the third column are all zeros, meaning z-linear constraint is deactivated) + * + * **beta** = + * | | + * | --| + * | 0 | + * | 0 | + * | 0 | + * | 0 | + * | 0 | + * | 0 | + * + * The last example involves the full specification of the desired end-effector motion (in this case, + * not necessarily zero accelerations), i.e. specification of constraints in all 6 **DOFs**: + * + * **alpha** = + * | | | | | | | + * | --| --| --| --| --| --| + * | 1 | 0 | 0 | 0 | 0 | 0 | + * | 0 | 1 | 0 | 0 | 0 | 0 | + * | 0 | 0 | 1 | 0 | 0 | 0 | + * | 0 | 0 | 0 | 1 | 0 | 0 | + * | 0 | 0 | 0 | 0 | 1 | 0 | + * | 0 | 0 | 0 | 0 | 0 | 1 | + * + * **beta** = **alpha^T * X_dotdot_N** + * + * Here, **N** stands for the index of the last robot's segment, end-effector (tool-tip). The reader should note + * that we can directly assign values (magnitudes) of the desired (task-defined) spatial acceleration **6 x 1** + * vector **X_dotdot_N** to the **6 x 1** vector of acceleration energy (**beta**) [3]. Even though physical + * dimensions (units) of these two vectors are not the same, the property of matrix **alpha** (it contains + * **unit** vectors), permits that we can assign values of desired accelerations to acceleration energy setpoints, + * in respective directions. Namely, each column of matrix **alpha** has the value of **1** in the respective + * direction in which constraint force works, thus it follows that the value of acceleration energy setpoint is + * the same as the value of Cartesian acceleration, in the respective direction. + * + * #### External Forces: f_ext + * + * This type of driver can be used for specifying **physical** (but not artificial, i.e. not task-introduced) + * Cartesian wrenches acting on each of the robot's segments [11]. Examples for a **physical** force on a segment can be: + * **i)** a known weight at the robot's gripper, for instance, a grasped cup or **ii)** a force from a human pushing + * the robot [5]. Note that the implementation of Vereshchagin solver in KDL expects the provided **f_ext** is + * expressed w.r.t. robot's base frame, which is in contrast to the case of KDL's RNE solver. + * + * ### Feed-Forward Joint Torques: ff_torque + * + * This type of motion driver can be used for specifying **physical** (but not artificial, i.e. not task-introduced) + * joint torques, for example, spring and/or damper-based torques (e.g. friction effects) in robot's joints [11]. + * + * Additional examples on using these input interfaces can be found in "../tests/solvertest.cpp": + * + * * VereshchaginTest() function - an example on how to use all interfaces of this solver for computing + * the solution to the Hybrid Dynamics (HD) problem. + * + * * FdAndVereshchaginSolversConsistencyTest() function - an example on how to only use this solver for + * computing the solution to the Featherstone's (i.e. Articulated Body Algorithm (ABA)) version of + * the Forward Dynamics (FD) problem. + * + * ### Solver's Output + * + * This recursive dynamics solver is computing several quantities that represent solutions to both, inverse and + * forward dynamics problems, or in other words solutions to the constrained hybrid dynamics problem. + * More specifically, the output interface of the original Popov-Vereshchagin algorithm consists of [3], [5]: + * + * * Magnitudes of constraint forces that act on the end-effector, denoted by the Lagrange multiplier **nu** + * in the solver's implementation and original solver's publication[2]. + * + * * Joint constraint torques required for achieving the desired (acceleration-constraints-defined) behavior of + * the robot: **constraint_torque**. These torques represent control commands that should be sent to robot's joint drivers. + * + * * Argument that defines the solution to the originally formulated optimization problem in **Gauss' principle**. + * In other words, the joint accelerations **q_dotdot** resulting from the total torque acting on each joint (**total_torque**), + * i.e. from the aforementioned constraint torques and all natural and external forces acting on the system. + * + * * The resulting and complete spatial accelerations of each segment in the kinematic chain: **X_dotdot** + * + * Furthermore, if necessary, a complete spatial vector of imposed constraint forces can be computed [3], + * from the following relation: **alpha * nu**. + * + * The reader should note that this **constraint_torque** is the **necessary** control command that a user is supposed + * to send to robot's joints, to achieve the motion that is computed (resolved) by the Popov-Vereshchagin solver. + * More specifically, here **constraint_torque** represent solution to the **Inverse Dynamics (ID)** problem. + * Nevertheless, the reason why a user is not supposed to use the **total_torque** values as the control commands + * for robot's joints, is the fact that the torque contributions that represent the difference between **total_torque** + * and **constraint_torque** already exist (act) on robot joints. More specifically, these **additional (residual)** + * contributions are produced on the joints by the already existing natural forces that act on the system [11]. + * + * On the other side, joint accelerations, namely **q_dotdot** provide solution to the **Forward Dynamics (FD)** problem + * and these quantities can be used for both control (integrate to joint positions/velocities) and simulation purposes [11]. + * + * ## PRACTICAL INSIGHTS/CONSIDERATIONS + * + * The Popov-Vereshchagin hybrid dynamics solver enables a user to achieve many types of operational space tasks [11]. + * In other words, various controllers can be implemented **around** the aforementioned interfaces of the algorithm. + * Examples can be controllers for hybrid force/position control, impedance control, etc. However, there are some practical + * insights about this algorithm that need to taken into account. + * + * ### Prioritizations between motion drivers (interfaces) + * + * The original derivation of this solver, which is considered in this library, prioritizes Cartesian acceleration constrains + * (specified for the end-effector segment) over other two motion drivers (Cartesian external wrenches and feedforward joint torques) [11]. + * In practice, this means the following: + * + * * If the external wrenches and/or feedforward joint torques contribute positively (i.e. assist) in producing Cartesian accelerations + * (specified via acceleration constraint interface), the Vereshchagin solver will take advantage of these forces to compute + * (acceleration-) energy optimal motions. + * + * * On the other hand, if the aforementioned external wrenches and/or feedforward joint torques contribute negatively (i.e. interfere) + * the Cartesian accelerations, the Vereshchagin solver will compensate all of those forces to correctly produce constrained + * accelerations of the end-effector. More specifically, additional torque commands will be computed under + * **constrained joint torques** (**ctrl_torques** in this implementation), to overcome those "disturbances". + * + * Nevertheless, the above-described prioritization can be changed (see [3] & [5] for more details) but those features are not implemented in KDL. + * + * ### Using the algorithm for solving forward dynamics (FD) problem + * + * The reader should note that the Popov-Vereshchagin solver represents an extension to the well-known forward-dynamics + * Articulated Body Algorithm (ABA) developed by Featherstone and described in [4] (moreover, Featherstone mentioned Vereshchagin solver + * in his book [4], page 117). This means that the Popov-Vereshchagin solver can also be purely used as this Articulated Body Algorithm + * forward-dynamics algorithm. In that case, it is necessary for the user to deactivate all Cartesian acceleration constraints (it is sufficient + * to set all elements in **alpha** matrix to zero) and proceed using other two interfaces as in the case of standard FD solver. More specifically, + * use **f_ext** input to define **physical** external wrenches acting on the robot's body (should be expressed w.r.t. robot's base frame) and + * **ff_torque** input to define command torques acting in robot's joints. The resulting robot's motion can be taken from **q_dotdot** and + * **X_dotdot** solver's outputs. + * + * Nevertheless, the Popov-Vereshchagin solver can also be used for solving more advanced forward dynamics problems, than those solved by ABA [4]. + * More specifically, if this solver is used in a certain simulation environment for the use-case of simulating robot behaviors, all three + * interfaces can be exploited for defining a more descriptive robot's state. Here, a user can exploit the Cartesian acceleration constraint + * interface to specify different constraints imposed on the end-effector, along with other interfaces, and simulate what would be the robot's + * behavior due to these constraints and environmental impacts. Here, the resulting robot's motion can, as well, be taken from **q_dotdot** and + * **X_dotdot** solver's outputs. For example, MuJoCo framework (see MuJoCo's documentation) also uses Gauss' principle of least constraint to + * simulate constraint forces in certain situations, however, there, final derivation of this principle in the software is different. However, in + * the case of this more advanced forward dynamics computations, the user needs to be aware of prioritizations between input interfaces + * (mentioned in "Prioritizations" section above) and internal policies on + * handling singularities (mentioned in "Singularities and matrix inversions" section below). + * + * ### Singularities and matrix inversions + * + * To find the minimal-energy solution to the Inverse Dynamics (ID) problem, i.e. find the Langrage multiplier **nu**, the solver needs to compute + * the inverse of a so-called "acceleration constraint coupling matrix"[3] in the balance equation before starting the third sweep. + * However, the robot's configuration has a direct impact on this matrix and its inversion. Namely, if the robot is in a singular configuration for + * the task specified via acceleration constraints, this matrix will become rank-deficient. This means that it is not possible to find a feasible + * solution for that particular end-effector's DOF that is (or DOFs that are) lost due to the singular configuration. In other words, it is not + * possible to find **constraint torques** that will satisfy imposed acceleration constraints in that DOF/DOFs. Nevertheless, in this situation, + * it is still possible to find the (energy-optimal) solution for other "non-singular" DOFs. For that reason, in KDL's implementation of the solver, + * matrix inverse is found by using the SVD technique to construct a pseudo inverse. Additionally, in this implementation, a control policy is + * introduced via the truncated-SVD method to deactivate, i.e. more specifically **ignore**, acceleration constraints for the DOFs that are lost due + * to robot's singular configuration. Of course, this is a choice (control policy), i.e. only one option for solving the singularity problem and + * producing safe joint commands. It is left for the user to explore other control policies (options) for this particular problem if of course, + * the user is not satisfied with the current control policy. + * + * ### Supported robot models + * + * KDL's current implementation of the Vereshchagin HD solver supports only robot chains that have equal number of joints and segments. + * Moreover, this implementation can only compute dynamics for **serial** type of chains, i.e. currently, **tree** robot structures are not supported + * in this solver. Nevertheless, the original solver's derivation has been extended in [3] to account for multiple motion constraints imposed + * on a **tree** robot structure. This extension does not only account for acceleration constraints imposed on multiple end-effectors but also for + * acceleration constraints imposed on more proximal segments. However, the above-mentioned extensions are currently not implemented in this version of KDL. + * + * ## REFERENCES + * + * [1] E. P. Popov, A. F. Vereshchagin, and S. L. Zenkevich, "Manipulyatsionnye roboty: Dinamika i algoritmy", Nauka, Moscow, 1978. + * + * [2] A. F. Vereshchagin, “Modelling and control of motion of manipulation robots”, Soviet Journal of Computer and Systems Sciences, vol. 27, pp. 29–38, 1989. + * + * [3] A. Shakhimardanov, “Composable robot motion stack: Implementing constrained hybrid dynamics using semantic models of kinematic chains”, PhD thesis, KU Leuven, 2015. + * + * [4] R. Featherstone, Rigid body dynamics algorithms. Springer, 2008. + * + * [5] S. Schneider and H. Bruyninckx, “Exploiting linearity in dynamics solvers for the design of composable robotic manipulation architectures”, in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2019. + * + * [6] H. Bruyninckx and O. Khatib, "Gauss’ principle and the dynamics of redundant and constrained manipulators", in IEEE International Conference on Robotics and Automation, 2000. + * + * [7] C. F. Gauß, "Über ein neues allgemeines Grundgesetz der Mechanik.", Journal für die reine und angewandte Mathematik, vol. 4, pp. 232–235, 1829. + * + * [8] A. F. Vereshchagin, “Computer simulation of the dynamics of complicated mechanisms of robot-manipulators”, Engineering Cybernetics, 12(6), pp. 65–70, 1974. + * + * [9] E. P. Popov, "Control of robots-manipulators", Engineering Cybernetics, 1974. + * + * [10] E. Ramm, “Principles of least action and of least constraint”, GAMM-Mitteilungen, vol. 34, pp. 164–182, 2011. + * + * [11] D. Vukcevic, "Lazy Robot Control by Relaxation of Motion and Force Constraints." Technical Report/Hochschule Bonn-Rhein-Sieg University of Applied Sciences, Department of Computer Science, 2020. + * + * @ingroup KinematicFamily + */ + +class ChainHdSolver_Vereshchagin : KDL::SolverI +{ + typedef std::vector Twists; + typedef std::vector Frames; + typedef Eigen::Matrix Vector6d; + typedef Eigen::Matrix Matrix6d; + typedef Eigen::Matrix Matrix6Xd; + +public: + /** + * Constructor for the solver, it will allocate all the necessary memory + * \param chain The kinematic chain to calculate the hybrid dynamics for. An internal copy will be made. + * \param root_acc The acceleration twist of the root segment to use during the calculation (usually contains gravity). + * Note: This solver takes gravity acceleration with opposite sign comparead to the KDL's FD and RNE solvers + * \param nc Number of constraints imposed on the robot's end-effector (maximum is 6). + */ + ChainHdSolver_Vereshchagin(const Chain& chain, const Twist &root_acc, const unsigned int nc); + + ~ChainHdSolver_Vereshchagin() + { + }; + + /** + * This method calculates joint space constraint torques and accelerations. + * It returns 0 when it succeeds, otherwise -1 or -2 for nonmatching matrix and array sizes. + * Input parameters: + * \param q The current joint positions + * \param q_dot The current joint velocities + * \param alpha The active constraint directions (unit constraint forces expressed w.r.t. robot's base frame) + * \param beta The acceleration energy setpoints (expressed w.r.t. above-defined unit constraint forces) + * \param f_ext The external forces (no gravity, it is given in root acceleration) on the segments + * \param ff_torques The feed-forward joint space torques + * + * Output parameters: + * \param q_dotdot The resulting joint accelerations + * \param constraint_torques The resulting joint constraint torques (what each joint feels due to the constraint forces acting on the end-effector) + * + * @return error/success code + */ + int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, const JntArray &ff_torques, JntArray &constraint_torques); + + /// @copydoc KDL::SolverI::updateInternalDataStructures + virtual void updateInternalDataStructures(); + + //Returns cartesian acceleration of links in base coordinates + void getTransformedLinkAcceleration(Twists& x_dotdot); + + // Returns total torque acting on each joint (constraints + nature + external forces) + void getTotalTorque(JntArray &total_tau); + + // Returns magnitude of the constraint forces acting on the end-effector: Lagrange Multiplier + void getContraintForceMagnitude(Eigen::VectorXd &nu_); + + /* + //Returns cartesian positions of links in base coordinates + void getLinkCartesianPose(Frames& x_base); + //Returns cartesian velocities of links in base coordinates + void getLinkCartesianVelocity(Twists& xDot_base); + //Returns cartesian acceleration of links in base coordinates + void getLinkCartesianAcceleration(Twists& xDotDot_base); + //Returns cartesian positions of links in link tip coordinates + void getLinkPose(Frames& x_local); + //Returns cartesian velocities of links in link tip coordinates + void getLinkVelocity(Twists& xDot_local); + //Returns cartesian acceleration of links in link tip coordinates + void getLinkAcceleration(Twists& xDotdot_local); + //Acceleration energy due to unit constraint forces at the end-effector + void getLinkUnitForceAccelerationEnergy(Eigen::MatrixXd& M); + //Acceleration energy due to arm configuration: bias force plus input joint torques + void getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G); + + void getLinkUnitForceMatrix(Matrix6Xd& E_tilde); + + void getLinkBiasForceMatrix(Wrenches& R_tilde); + + void getJointBiasAcceleration(JntArray &bias_q_dotdot); + */ +private: + /** + * This method calculates all cartesian space poses, twists, bias accelerations. + * External forces are also taken into account in this outward sweep. + */ + void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext); + /** + * This method is a force balance sweep. It calculates articulated body inertias and bias forces. + * Additionally, acceleration energies generated by bias forces and unit forces are calculated here. + */ + void downwards_sweep(const Jacobian& alfa, const JntArray& ff_torques); + /** + * This method calculates constraint force magnitudes. + * + */ + void constraint_calculation(const JntArray& beta); + /** + * This method puts all acceleration contributions (constraint, bias, nullspace and parent accelerations) together. + * + */ + void final_upwards_sweep(JntArray &q_dotdot, JntArray &constraint_torques); + +private: + const Chain& chain; + unsigned int nj; + unsigned int ns; + unsigned int nc; + Twist acc_root; + Jacobian alfa_N; + Jacobian alfa_N2; + Eigen::MatrixXd M_0_inverse; + Eigen::MatrixXd Um; + Eigen::MatrixXd Vm; + JntArray beta_N; + Eigen::VectorXd nu; + Eigen::VectorXd nu_sum; + Eigen::VectorXd Sm; + Eigen::VectorXd tmpm; + Eigen::VectorXd total_torques; // all the contributions that are felt at the joint: constraints + nature + external forces + Wrench qdotdot_sum; + Frame F_total; + + struct segment_info + { + Frame F; //local pose with respect to previous link in segments coordinates + Frame F_base; // pose of a segment in root coordinates + Twist Z; //Unit twist + Twist v; //twist + Twist acc; //acceleration twist + Wrench U; //wrench p of the bias forces (in cartesian space) + Wrench R; //wrench p of the bias forces + Wrench R_tilde; //vector of wrench p of the bias forces (new) in matrix form + Twist C; //constraint + Twist A; //constraint + ArticulatedBodyInertia H; //I (expressed in 6*6 matrix) + ArticulatedBodyInertia P; //I (expressed in 6*6 matrix) + ArticulatedBodyInertia P_tilde; //I (expressed in 6*6 matrix) + Wrench PZ; //vector U[i] = I_A[i]*S[i] + Wrench PC; //vector E[i] = I_A[i]*c[i] + double D; //vector D[i] = S[i]^T*U[i] + Matrix6Xd E; //matrix with virtual unit constraint force due to acceleration constraints + Matrix6Xd E_tilde; + Eigen::MatrixXd M; //acceleration energy already generated at link i + Eigen::VectorXd G; //magnitude of the constraint forces already generated at link i + Eigen::VectorXd EZ; //K[i] = Etiltde'*Z + double nullspaceAccComp; //Azamat: constribution of joint space u[i] forces to joint space acceleration + double constAccComp; //Azamat: constribution of joint space constraint forces to joint space acceleration + double biasAccComp; //Azamat: constribution of joint space bias forces to joint space acceleration + double totalBias; //Azamat: R+PC (centrepital+coriolis) in joint subspace + double u; //vector u[i] = torques(i) - S[i]^T*(p_A[i] + I_A[i]*C[i]) in joint subspace. Azamat: In code u[i] = torques(i) - s[i].totalBias + + segment_info(unsigned int nc): + D(0),nullspaceAccComp(0),constAccComp(0),biasAccComp(0),totalBias(0),u(0) + { + E.resize(6, nc); + E_tilde.resize(6, nc); + G.resize(nc); + M.resize(nc, nc); + EZ.resize(nc); + E.setZero(); + E_tilde.setZero(); + M.setZero(); + G.setZero(); + EZ.setZero(); + }; + }; + + std::vector > results; + +}; +} + +#endif // KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP diff --git a/orocos_kdl/src/chainidsolver_recursive_newton_euler.cpp b/orocos_kdl/src/chainidsolver_recursive_newton_euler.cpp index 9a63fb5a1..b93a59ce5 100644 --- a/orocos_kdl/src/chainidsolver_recursive_newton_euler.cpp +++ b/orocos_kdl/src/chainidsolver_recursive_newton_euler.cpp @@ -1,8 +1,8 @@ -// Copyright (C) 2009 Ruben Smits +// Copyright (C) 2009 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -23,7 +23,7 @@ #include "frames_io.hpp" namespace KDL{ - + ChainIdSolver_RNE::ChainIdSolver_RNE(const Chain& chain_,Vector grav): chain(chain_),nj(chain.getNrOfJoints()),ns(chain.getNrOfSegments()), X(ns),S(ns),v(ns),a(ns),f(ns) @@ -54,17 +54,17 @@ namespace KDL{ //Sweep from root to leaf for(unsigned int i=0;i=0;i--){ - if(chain.getSegment(i).getJoint().getType()!=Joint::None) - torques(j--)=dot(S[i],f[i]); + if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) { + torques(j)=dot(S[i],f[i]); + torques(j)+=chain.getSegment(i).getJoint().getInertia()*q_dotdot(j); // add torque from joint inertia + --j; + } if(i!=0) f[i-1]=f[i-1]+X[i]*f[i]; } diff --git a/orocos_kdl/src/chainidsolver_vereshchagin.cpp b/orocos_kdl/src/chainidsolver_vereshchagin.cpp index 880503a53..31c8f1bae 100644 --- a/orocos_kdl/src/chainidsolver_vereshchagin.cpp +++ b/orocos_kdl/src/chainidsolver_vereshchagin.cpp @@ -1,8 +1,10 @@ -// Copyright (C) 2009, 2011 +// Copyright (C) 2009 Ruben Smits // Version: 1.0 -// Author: Ruben Smits, Herman Bruyninckx, Azamat Shakhimardanov -// Maintainer: Ruben Smits, Azamat Shakhimardanov +// Author: Ruben Smits +// Author: Herman Bruyninckx +// Author: Azamat Shakhimardanov +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -20,461 +22,15 @@ // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA #include "chainidsolver_vereshchagin.hpp" -#include "frames_io.hpp" -#include "utilities/svd_eigen_HH.hpp" +#include namespace KDL { -using namespace Eigen; -ChainIdSolver_Vereshchagin::ChainIdSolver_Vereshchagin(const Chain& chain_, Twist root_acc, unsigned int _nc) : - chain(chain_), nj(chain.getNrOfJoints()), ns(chain.getNrOfSegments()), nc(_nc), - results(ns + 1, segment_info(nc)) +ChainIdSolver_Vereshchagin::ChainIdSolver_Vereshchagin(const Chain& chain_, const Twist &root_acc, const unsigned int nc_) : ChainHdSolver_Vereshchagin(chain_, root_acc, nc_) { - acc_root = root_acc; - - //Provide the necessary memory for computing the inverse of M0 - nu_sum.resize(nc); - M_0_inverse.resize(nc, nc); - Um = MatrixXd::Identity(nc, nc); - Vm = MatrixXd::Identity(nc, nc); - Sm = VectorXd::Ones(nc); - tmpm = VectorXd::Ones(nc); -} - -void ChainIdSolver_Vereshchagin::updateInternalDataStructures() { - ns = chain.getNrOfSegments(); - results.resize(ns+1,segment_info(nc)); -} - -int ChainIdSolver_Vereshchagin::CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques) -{ - nj = chain.getNrOfJoints(); - if(ns != chain.getNrOfSegments()) - return (error = E_NOT_UP_TO_DATE); - //Check sizes always - if (q.rows() != nj || q_dot.rows() != nj || q_dotdot.rows() != nj || torques.rows() != nj || f_ext.size() != ns) - return (error = E_SIZE_MISMATCH); - if (alfa.columns() != nc || beta.rows() != nc) - return (error = E_SIZE_MISMATCH); - //do an upward recursion for position and velocities - this->initial_upwards_sweep(q, q_dot, q_dotdot, f_ext); - //do an inward recursion for inertia, forces and constraints - this->downwards_sweep(alfa, torques); - //Solve for the constraint forces - this->constraint_calculation(beta); - //do an upward recursion to propagate the result - this->final_upwards_sweep(q_dotdot, torques); - return (error = E_NOERROR); -} - -void ChainIdSolver_Vereshchagin::initial_upwards_sweep(const JntArray &q, const JntArray &qdot, const JntArray &qdotdot, const Wrenches& f_ext) -{ - //if (q.rows() != nj || qdot.rows() != nj || qdotdot.rows() != nj || f_ext.size() != ns) - // return -1; - - unsigned int j = 0; - F_total = Frame::Identity(); - for (unsigned int i = 0; i < ns; i++) - { - //Express everything in the segments reference frame (body coordinates) - //which is at the segments tip, i.e. where the next joint is attached. - - //Calculate segment properties: X,S,vj,cj - const Segment& segment = chain.getSegment(i); - segment_info& s = results[i + 1]; - //The pose between the joint root and the segment tip (tip expressed in joint root coordinates) - s.F = segment.pose(q(j)); //X pose of each link in link coord system - - F_total = F_total * s.F; //X pose of the each link in root coord system - s.F_base = F_total; //X pose of the each link in root coord system for getter functions - - //The velocity due to the joint motion of the segment expressed in the segments reference frame (tip) - Twist vj = s.F.M.Inverse(segment.twist(q(j), qdot(j))); //XDot of each link - //Twist aj = s.F.M.Inverse(segment.twist(q(j), qdotdot(j))); //XDotDot of each link - - //The unit velocity due to the joint motion of the segment expressed in the segments reference frame (tip) - s.Z = s.F.M.Inverse(segment.twist(q(j), 1.0)); - //Put Z in the joint root reference frame: - s.Z = s.F * s.Z; - - //The total velocity of the segment expressed in the segments reference frame (tip) - if (i != 0) - { - s.v = s.F.Inverse(results[i].v) + vj; // recursive velocity of each link in segment frame - //s.A=s.F.Inverse(results[i].A)+aj; - s.A = s.F.M.Inverse(results[i].A); - } - else - { - s.v = vj; - s.A = s.F.M.Inverse(acc_root); - } - //c[i] = cj + v[i]xvj (remark: cj=0, since our S is not time dependent in local coordinates) - //The velocity product acceleration - //std::cout << i << " Initial upward" << s.v << std::endl; - s.C = s.v*vj; //This is a cross product: cartesian space BIAS acceleration in local link coord. - //Put C in the joint root reference frame - s.C = s.F * s.C; //+F_total.M.Inverse(acc_root)); - //The rigid body inertia of the segment, expressed in the segments reference frame (tip) - s.H = segment.getInertia(); - - //wrench of the rigid body bias forces and the external forces on the segment (in body coordinates, tip) - //external forces are taken into account through s.U. - Wrench FextLocal = F_total.M.Inverse() * f_ext[i]; - s.U = s.v * (s.H * s.v) - FextLocal; //f_ext[i]; - if (segment.getJoint().getType() != Joint::None) - j++; - } - -} - -void ChainIdSolver_Vereshchagin::downwards_sweep(const Jacobian& alfa, const JntArray &torques) -{ - int j = nj - 1; - for (int i = ns; i >= 0; i--) - { - //Get a handle for the segment we are working on. - segment_info& s = results[i]; - //For segment N, - //tilde is in the segment refframe (tip, not joint root) - //without tilde is at the joint root (the childs tip!!!) - //P_tilde is the articulated body inertia - //R_tilde is the sum of external and coriolis/centrifugal forces - //M is the (unit) acceleration energy already generated at link i - //G is the (unit) magnitude of the constraint forces at link i - //E are the (unit) constraint forces due to the constraints - if (i == (int)ns) - { - s.P_tilde = s.H; - s.R_tilde = s.U; - s.M.setZero(); - s.G.setZero(); - //changeBase(alfa_N,F_total.M.Inverse(),alfa_N2); - for (unsigned int r = 0; r < 3; r++) - for (unsigned int c = 0; c < nc; c++) - { - //copy alfa constrain force matrix in E~ - s.E_tilde(r, c) = alfa(r + 3, c); - s.E_tilde(r + 3, c) = alfa(r, c); - } - //Change the reference frame of alfa to the segmentN tip frame - //F_Total holds end effector frame, if done per segment bases then constraints could be extended to all segments - Rotation base_to_end = F_total.M.Inverse(); - for (unsigned int c = 0; c < nc; c++) - { - Wrench col(Vector(s.E_tilde(3, c), s.E_tilde(4, c), s.E_tilde(5, c)), - Vector(s.E_tilde(0, c), s.E_tilde(1, c), s.E_tilde(2, c))); - col = base_to_end*col; - s.E_tilde.col(c) << Vector3d::Map(col.torque.data), Vector3d::Map(col.force.data); - } - } - else - { - //For all others: - //Everything should expressed in the body coordinates of segment i - segment_info& child = results[i + 1]; - //Copy PZ into a vector so we can do matrix manipulations, put torques above forces - Vector6d vPZ; - vPZ << Vector3d::Map(child.PZ.torque.data), Vector3d::Map(child.PZ.force.data); - Matrix6d PZDPZt; - PZDPZt.noalias() = vPZ * vPZ.transpose(); - PZDPZt /= child.D; - - //equation a) (see Vereshchagin89) PZDPZt=[I,H;H',M] - //Azamat:articulated body inertia as in Featherstone (7.19) - s.P_tilde = s.H + child.P - ArticulatedBodyInertia(PZDPZt.bottomRightCorner<3,3>(), PZDPZt.topRightCorner<3,3>(), PZDPZt.topLeftCorner<3,3>()); - //equation b) (see Vereshchagin89) - //Azamat: bias force as in Featherstone (7.20) - s.R_tilde = s.U + child.R + child.PC + (child.PZ / child.D) * child.u; - //equation c) (see Vereshchagin89) - s.E_tilde = child.E; - - //Azamat: equation (c) right side term - s.E_tilde.noalias() -= (vPZ * child.EZ.transpose()) / child.D; - - //equation d) (see Vereshchagin89) - s.M = child.M; - //Azamat: equation (d) right side term - s.M.noalias() -= (child.EZ * child.EZ.transpose()) / child.D; - - //equation e) (see Vereshchagin89) - s.G = child.G; - Twist CiZDu = child.C + (child.Z / child.D) * child.u; - Vector6d vCiZDu; - vCiZDu << Vector3d::Map(CiZDu.rot.data), Vector3d::Map(CiZDu.vel.data); - s.G.noalias() += child.E.transpose() * vCiZDu; - } - if (i != 0) - { - //Transform all results to joint root coordinates of segment i (== body coordinates segment i-1) - //equation a) - s.P = s.F * s.P_tilde; - //equation b) - s.R = s.F * s.R_tilde; - //equation c), in matrix: torques above forces, so switch and switch back - for (unsigned int c = 0; c < nc; c++) - { - Wrench col(Vector(s.E_tilde(3, c), s.E_tilde(4, c), s.E_tilde(5, c)), - Vector(s.E_tilde(0, c), s.E_tilde(1, c), s.E_tilde(2, c))); - col = s.F*col; - s.E.col(c) << Vector3d::Map(col.torque.data), Vector3d::Map(col.force.data); - } - - //needed for next recursion - s.PZ = s.P * s.Z; - s.D = dot(s.Z, s.PZ); - s.PC = s.P * s.C; - - //u=(Q-Z(R+PC)=sum of external forces along the joint axes, - //R are the forces coming from the children, - //Q is taken zero (do we need to take the previous calculated torques? - - //projection of coriolis and centrepital forces into joint subspace (0 0 Z) - s.totalBias = -dot(s.Z, s.R + s.PC); - s.u = torques(j) + s.totalBias; - - //Matrix form of Z, put rotations above translations - Vector6d vZ; - vZ << Vector3d::Map(s.Z.rot.data), Vector3d::Map(s.Z.vel.data); - s.EZ.noalias() = s.E.transpose() * vZ; - - if (chain.getSegment(i - 1).getJoint().getType() != Joint::None) - j--; - } - } -} - -void ChainIdSolver_Vereshchagin::constraint_calculation(const JntArray& beta) -{ - //equation f) nu = M_0_inverse*(beta_N - E0_tilde`*acc0 - G0) - //M_0_inverse, always nc*nc symmetric matrix - //std::cout<<"M0: "< axis torque/force - double constraint_torque = -dot(s.Z, constraint_force); - //The result should be the torque at this joint - - torques(j) = constraint_torque; - //s.constAccComp = torques(j) / s.D; - s.constAccComp = constraint_torque / s.D; - s.nullspaceAccComp = s.u / s.D; - //total joint space acceleration resulting from accelerations of parent joints, constraint forces and - // nullspace forces. - q_dotdot(j) = (s.nullspaceAccComp + parentAccComp + s.constAccComp); - s.acc = s.F.Inverse(a_p + s.Z * q_dotdot(j) + s.C);//returns acceleration in link distal tip coordinates. For use needs to be transformed - if (chain.getSegment(i - 1).getJoint().getType() != Joint::None) - j++; - } -} - -/* -void ChainIdSolver_Vereshchagin::getLinkCartesianPose(Frames& x_base) -{ - for (int i = 0; i < ns; i++) - { - x_base[i] = results[i + 1].F_base; - } - return; + std::cout << "ChainIdSolver_Vereshchagin is renamed to ChainHdSolver_Vereshchagin in version 1.5. The old name will be dropped in version 1.6" << std::endl; } -void ChainIdSolver_Vereshchagin::getLinkCartesianVelocity(Twists& xDot_base) -{ - - for (int i = 0; i < ns; i++) - { - xDot_base[i] = results[i + 1].F_base.M * results[i + 1].v; - } - return; -} - -void ChainIdSolver_Vereshchagin::getLinkCartesianAcceleration(Twists& xDotDot_base) -{ - - for (int i = 0; i < ns; i++) - { - xDotDot_base[i] = results[i + 1].F_base.M * results[i + 1].acc; - //std::cout << "XDotDot_base[i] " << xDotDot_base[i] << std::endl; - } - return; -} - -void ChainIdSolver_Vereshchagin::getLinkPose(Frames& x_local) -{ - for (int i = 0; i < ns; i++) - { - x_local[i] = results[i + 1].F; - } - return; -} - -void ChainIdSolver_Vereshchagin::getLinkVelocity(Twists& xDot_local) -{ - for (int i = 0; i < ns; i++) - { - xDot_local[i] = results[i + 1].v; - } - return; - -} - -void ChainIdSolver_Vereshchagin::getLinkAcceleration(Twists& xDotdot_local) -{ - for (int i = 0; i < ns; i++) - { - xDotdot_local[i] = results[i + 1].acc; - } - return; - -} - -void ChainIdSolver_Vereshchagin::getJointBiasAcceleration(JntArray& bias_q_dotdot) -{ - for (int i = 0; i < ns; i++) - { - //this is only force - double tmp = results[i + 1].totalBias; - //this is acceleration - bias_q_dotdot(i) = tmp / results[i + 1].D; - - //s.totalBias = - dot(s.Z, s.R + s.PC); - //std::cout << "totalBiasAccComponent" << i << ": " << bias_q_dotdot(i) << std::endl; - //bias_q_dotdot(i) = s.totalBias/s.D - - } - return; - -} - -void ChainIdSolver_Vereshchagin::getJointConstraintAcceleration(JntArray& constraint_q_dotdot) -{ - for (int i = 0; i < ns; i++) - { - constraint_q_dotdot(i) = results[i + 1].constAccComp; - //double tmp = results[i + 1].u; - //s.u = torques(j) + s.totalBias; - // std::cout << "s.constraintAccComp" << i << ": " << results[i+1].constAccComp << std::endl; - //nullspace_q_dotdot(i) = s.u/s.D - - } - return; - - -} - -//Check the name it does not seem to be appropriate - -void ChainIdSolver_Vereshchagin::getJointNullSpaceAcceleration(JntArray& nullspace_q_dotdot) -{ - for (int i = 0; i < ns; i++) - { - nullspace_q_dotdot(i) = results[i + 1].nullspaceAccComp; - //double tmp = results[i + 1].u; - //s.u = torques(j) + s.totalBias; - //std::cout << "s.nullSpaceAccComp" << i << ": " << results[i+1].nullspaceAccComp << std::endl; - //nullspace_q_dotdot(i) = s.u/s.D - - } - return; - - } - -//This is not only a bias force energy but also includes generalized forces -//change type of parameter G -//this method should return array of G's - -void ChainIdSolver_Vereshchagin::getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G) -{ - for (int i = 0; i < ns; i++) - { - G = results[i + 1].G; - //double tmp = results[i + 1].u; - //s.u = torques(j) + s.totalBias; - //std::cout << "s.G " << i << ": " << results[i+1].G << std::endl; - //nullspace_q_dotdot(i) = s.u/s.D - - } - return; - -} - -//this method should return array of R's - -void ChainIdSolver_Vereshchagin::getLinkBiasForceMatrix(Wrenches& R_tilde) -{ - for (int i = 0; i < ns; i++) - { - R_tilde[i] = results[i + 1].R_tilde; - //Azamat: bias force as in Featherstone (7.20) - //s.R_tilde = s.U + child.R + child.PC + child.PZ / child.D * child.u; - std::cout << "s.R_tilde " << i << ": " << results[i + 1].R_tilde << std::endl; - } - return; -} - -*/ - -}//namespace diff --git a/orocos_kdl/src/chainidsolver_vereshchagin.hpp b/orocos_kdl/src/chainidsolver_vereshchagin.hpp index 5881e3de1..0f5d736f4 100644 --- a/orocos_kdl/src/chainidsolver_vereshchagin.hpp +++ b/orocos_kdl/src/chainidsolver_vereshchagin.hpp @@ -1,8 +1,10 @@ -// Copyright (C) 2009, 2011 +// Copyright (C) 2009 Ruben Smits // Version: 1.0 -// Author: Ruben Smits, Herman Bruyninckx, Azamat Shakhimardanov -// Maintainer: Ruben Smits, Azamat Shakhimardanov +// Author: Ruben Smits +// Author: Herman Bruyninckx +// Author: Azamat Shakhimardanov +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -22,172 +24,17 @@ #ifndef KDL_CHAINIDSOLVER_VERESHCHAGIN_HPP #define KDL_CHAINIDSOLVER_VERESHCHAGIN_HPP -#include "chainidsolver.hpp" -#include "frames.hpp" -#include "articulatedbodyinertia.hpp" - -#include +#include "chainhdsolver_vereshchagin.hpp" namespace KDL { -/** - * \brief Dynamics calculations by constraints based on Vereshchagin 1989. - * for a chain. This class creates instance of hybrid dynamics solver. - * The solver calculates total joint space accelerations in a chain when a constraint force(s) is applied - * to the chain's end-effector (task space/cartesian space). - */ -class ChainIdSolver_Vereshchagin : KDL::SolverI +class ChainIdSolver_Vereshchagin : public ChainHdSolver_Vereshchagin { - typedef std::vector Twists; - typedef std::vector Frames; - typedef Eigen::Matrix Vector6d; - typedef Eigen::Matrix Matrix6d; - typedef Eigen::Matrix Matrix6Xd; - public: - /** - * Constructor for the solver, it will allocate all the necessary memory - * \param chain The kinematic chain to calculate the inverse dynamics for, an internal copy will be made. - * \param root_acc The acceleration vector of the root to use during the calculation.(most likely contains gravity) - * - */ - ChainIdSolver_Vereshchagin(const Chain& chain, Twist root_acc, unsigned int nc); - - ~ChainIdSolver_Vereshchagin() - { - }; - - /** - * This method calculates joint space constraint torques and total joint space acceleration. - * It returns 0 when it succeeds, otherwise -1 or -2 for unmatching matrix and array sizes. - * Input parameters; - * \param q The current joint positions - * \param q_dot The current joint velocities - * \param f_ext The external forces (no gravity, it is given in root acceleration) on the segments. - * Output parameters: - * \param q_dotdot The joint accelerations - * \param torques the resulting constraint torques for the joints - * - * @return error/success code - */ - int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques); - - /// @copydoc KDL::SolverI::updateInternalDataStructures - virtual void updateInternalDataStructures(); - /* - //Returns cartesian positions of links in base coordinates - void getLinkCartesianPose(Frames& x_base); - //Returns cartesian velocities of links in base coordinates - void getLinkCartesianVelocity(Twists& xDot_base); - //Returns cartesian acceleration of links in base coordinates - void getLinkCartesianAcceleration(Twists& xDotDot_base); - //Returns cartesian positions of links in link tip coordinates - void getLinkPose(Frames& x_local); - //Returns cartesian velocities of links in link tip coordinates - void getLinkVelocity(Twists& xDot_local); - //Returns cartesian acceleration of links in link tip coordinates - void getLinkAcceleration(Twists& xDotdot_local); - //Acceleration energy due to unit constraint forces at the end-effector - void getLinkUnitForceAccelerationEnergy(Eigen::MatrixXd& M); - //Acceleration energy due to arm configuration: bias force plus input joint torques - void getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G); - - void getLinkUnitForceMatrix(Matrix6Xd& E_tilde); - - void getLinkBiasForceMatrix(Wrenches& R_tilde); - - void getJointBiasAcceleration(JntArray &bias_q_dotdot); - */ -private: - /** - * This method calculates all cartesian space poses, twists, bias accelerations. - * External forces are also taken into account in this outward sweep. - */ - void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext); - /** - * This method is a force balance sweep. It calculates articulated body inertias and bias forces. - * Additionally, acceleration energies generated by bias forces and unit forces are calculated here. - */ - void downwards_sweep(const Jacobian& alfa, const JntArray& torques); - /** - * This method calculates constraint force magnitudes. - * - */ - void constraint_calculation(const JntArray& beta); - /** - * This method puts all acceleration contributions (constraint, bias, nullspace and parent accelerations) together. - * - */ - void final_upwards_sweep(JntArray &q_dotdot, JntArray &torques); - -private: - const Chain& chain; - unsigned int nj; - unsigned int ns; - unsigned int nc; - Twist acc_root; - Jacobian alfa_N; - Jacobian alfa_N2; - Eigen::MatrixXd M_0_inverse; - Eigen::MatrixXd Um; - Eigen::MatrixXd Vm; - JntArray beta_N; - Eigen::VectorXd nu; - Eigen::VectorXd nu_sum; - Eigen::VectorXd Sm; - Eigen::VectorXd tmpm; - Wrench qdotdot_sum; - Frame F_total; - - struct segment_info - { - Frame F; //local pose with respect to previous link in segments coordinates - Frame F_base; // pose of a segment in root coordinates - Twist Z; //Unit twist - Twist v; //twist - Twist acc; //acceleration twist - Wrench U; //wrench p of the bias forces (in cartesian space) - Wrench R; //wrench p of the bias forces - Wrench R_tilde; //vector of wrench p of the bias forces (new) in matrix form - Twist C; //constraint - Twist A; //constraint - ArticulatedBodyInertia H; //I (expressed in 6*6 matrix) - ArticulatedBodyInertia P; //I (expressed in 6*6 matrix) - ArticulatedBodyInertia P_tilde; //I (expressed in 6*6 matrix) - Wrench PZ; //vector U[i] = I_A[i]*S[i] - Wrench PC; //vector E[i] = I_A[i]*c[i] - double D; //vector D[i] = S[i]^T*U[i] - Matrix6Xd E; //matrix with virtual unit constraint force due to acceleration constraints - Matrix6Xd E_tilde; - Eigen::MatrixXd M; //acceleration energy already generated at link i - Eigen::VectorXd G; //magnitude of the constraint forces already generated at link i - Eigen::VectorXd EZ; //K[i] = Etiltde'*Z - double nullspaceAccComp; //Azamat: constribution of joint space u[i] forces to joint space acceleration - double constAccComp; //Azamat: constribution of joint space constraint forces to joint space acceleration - double biasAccComp; //Azamat: constribution of joint space bias forces to joint space acceleration - double totalBias; //Azamat: R+PC (centrepital+coriolis) in joint subspace - double u; //vector u[i] = torques(i) - S[i]^T*(p_A[i] + I_A[i]*C[i]) in joint subspace. Azamat: In code u[i] = torques(i) - s[i].totalBias - - segment_info(unsigned int nc): - D(0),nullspaceAccComp(0),constAccComp(0),biasAccComp(0),totalBias(0),u(0) - { - E.resize(6, nc); - E_tilde.resize(6, nc); - G.resize(nc); - M.resize(nc, nc); - EZ.resize(nc); - E.setZero(); - E_tilde.setZero(); - M.setZero(); - G.setZero(); - EZ.setZero(); - }; - }; - - std::vector > results; - + ChainIdSolver_Vereshchagin(const Chain& chain, const Twist &root_acc, const unsigned int nc); }; + } -#endif +#endif // KDL_CHAINIDSOLVER_VERESHCHAGIN_HPP diff --git a/orocos_kdl/src/chainiksolverpos_lma.cpp b/orocos_kdl/src/chainiksolverpos_lma.cpp index 8b11ae573..2fd5588b6 100644 --- a/orocos_kdl/src/chainiksolverpos_lma.cpp +++ b/orocos_kdl/src/chainiksolverpos_lma.cpp @@ -144,7 +144,7 @@ void ChainIkSolverPos_LMA::compute_fwdpos(const VectorXq& q) { T_base_head = Frame::Identity(); // frame w.r.t. base of head for (unsigned int i=0;i fksolver.JntToCart(q_out,f) ) - return (error = E_FKSOLVERPOS_FAILED); - delta_twist = diff(f,p_in); - const int rc = iksolver.CartToJnt(q_out,delta_twist,delta_q); - if (E_NOERROR > rc) - return (error = E_IKSOLVER_FAILED); - // we chose to continue if the child solver returned a positive - // "error", which may simply indicate a degraded solution - Add(q_out,delta_q,q_out); - if(Equal(delta_twist,Twist::Zero(),eps)) - // converged, but possibly with a degraded solution - return (rc > E_NOERROR ? E_DEGRADED : E_NOERROR); - } - return (error = E_MAX_ITERATIONS_EXCEEDED); // failed to converge + unsigned int i; + for(i=0;i fksolver.JntToCart(q_out,f) ) + return (error = E_FKSOLVERPOS_FAILED); + delta_twist = diff(f,p_in); + const int rc = iksolver.CartToJnt(q_out,delta_twist,delta_q); + if (E_NOERROR > rc) + return (error = E_IKSOLVER_FAILED); + // we chose to continue if the child solver returned a positive + // "error", which may simply indicate a degraded solution + Add(q_out,delta_q,q_out); + if(Equal(delta_twist,Twist::Zero(),eps)) + // converged, but possibly with a degraded solution + return (rc > E_NOERROR ? E_DEGRADED : E_NOERROR); + } + return (error = E_MAX_ITERATIONS_EXCEEDED); // failed to converge } ChainIkSolverPos_NR::~ChainIkSolverPos_NR() diff --git a/orocos_kdl/src/chainiksolverpos_nr_jl.cpp b/orocos_kdl/src/chainiksolverpos_nr_jl.cpp index e5ec40e05..bb30c45e1 100644 --- a/orocos_kdl/src/chainiksolverpos_nr_jl.cpp +++ b/orocos_kdl/src/chainiksolverpos_nr_jl.cpp @@ -67,37 +67,37 @@ namespace KDL if(nj != q_init.rows() || nj != q_out.rows() || nj != q_min.rows() || nj != q_max.rows()) return (error = E_SIZE_MISMATCH); - q_out = q_init; + q_out = q_init; - unsigned int i; - for(i=0;i q_max(j)) - q_out(j) = q_max(j); - } + for(unsigned int j=0; j q_max(j)) + q_out(j) = q_max(j); } + } - if(i!=maxiter) - return (error = E_NOERROR); - else - return (error = E_MAX_ITERATIONS_EXCEEDED); + if(i!=maxiter) + return (error = E_NOERROR); + else + return (error = E_MAX_ITERATIONS_EXCEEDED); } int ChainIkSolverPos_NR_JL::setJointLimits(const JntArray& q_min_in, const JntArray& q_max_in) { diff --git a/orocos_kdl/src/chainiksolvervel_pinv.cpp b/orocos_kdl/src/chainiksolvervel_pinv.cpp index 8b5d70ec6..4ecc0cc49 100644 --- a/orocos_kdl/src/chainiksolvervel_pinv.cpp +++ b/orocos_kdl/src/chainiksolvervel_pinv.cpp @@ -25,8 +25,8 @@ namespace KDL { ChainIkSolverVel_pinv::ChainIkSolverVel_pinv(const Chain& _chain,double _eps,int _maxiter): chain(_chain), - nj(chain.getNrOfJoints()), jnt2jac(chain), + nj(chain.getNrOfJoints()), jac(nj), svd(jac), U(6,JntArray(nj)), diff --git a/orocos_kdl/src/chainiksolvervel_pinv.hpp b/orocos_kdl/src/chainiksolvervel_pinv.hpp index 8ddc32349..63a6f9c4c 100644 --- a/orocos_kdl/src/chainiksolvervel_pinv.hpp +++ b/orocos_kdl/src/chainiksolvervel_pinv.hpp @@ -77,7 +77,7 @@ namespace KDL * not (yet) implemented. * */ - virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;}; + virtual int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/){return (error = E_NOT_IMPLEMENTED);}; /** * Retrieve the number of singular values of the jacobian that are < eps; diff --git a/orocos_kdl/src/chainiksolvervel_pinv_givens.cpp b/orocos_kdl/src/chainiksolvervel_pinv_givens.cpp index e5ab28408..fbf102b40 100644 --- a/orocos_kdl/src/chainiksolvervel_pinv_givens.cpp +++ b/orocos_kdl/src/chainiksolvervel_pinv_givens.cpp @@ -30,17 +30,16 @@ namespace KDL jnt2jac(chain), jac(nj), transpose(nj>6),toggle(true), - m(max(6,nj)), - n(min(6,nj)), + m(static_cast(max(6,nj))), + n(static_cast(min(6,nj))), jac_eigen(m,n), - U(MatrixXd::Identity(m,m)), - V(MatrixXd::Identity(n,n)), + U(Eigen::MatrixXd::Identity(m,m)), + V(Eigen::MatrixXd::Identity(n,n)), B(m,n), S(n), tempi(m), - tempj(m), - UY(VectorXd::Zero(6)), - SUY(VectorXd::Zero(nj)), + UY(Eigen::VectorXd::Zero(6)), + SUY(Eigen::VectorXd::Zero(nj)), qdot_eigen(nj), v_in_eigen(6) { @@ -51,16 +50,15 @@ namespace KDL jnt2jac.updateInternalDataStructures(); jac.resize(nj); transpose = (nj > 6); - m = max(6,nj); - n = min(6,nj); + m = static_cast(max(6,nj)); + n = static_cast(min(6,nj)); jac_eigen.conservativeResize(m,n); - U.conservativeResizeLike(MatrixXd::Identity(m,m)); - V.conservativeResizeLike(MatrixXd::Identity(n,n)); + U.conservativeResizeLike(Eigen::MatrixXd::Identity(m,m)); + V.conservativeResizeLike(Eigen::MatrixXd::Identity(n,n)); B.conservativeResize(m,n); S.conservativeResize(n); tempi.conservativeResize(m); - tempj.conservativeResize(n); - SUY.conservativeResizeLike(VectorXd::Zero(nj)); + SUY.conservativeResizeLike(Eigen::VectorXd::Zero(nj)); qdot_eigen.conservativeResize(nj); } @@ -93,8 +91,7 @@ namespace KDL else jac_eigen(i,j)=jac(i,j); } - int ret = svd_eigen_Macie(jac_eigen,U,S,V,B,tempi,1e-15,toggle); - //std::cout<<"# sweeps: "< -using namespace Eigen; - namespace KDL { /** @@ -30,10 +28,6 @@ namespace KDL * * @param chain the chain to calculate the inverse velocity * kinematics for - * @param eps if a singular value is below this value, its - * inverse is set to zero, default: 0.00001 - * @param maxiter maximum iterations for the svd calculation, - * default: 150 * */ explicit ChainIkSolverVel_pinv_givens(const Chain& chain); @@ -44,7 +38,7 @@ namespace KDL * not (yet) implemented. * */ - virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return (error = E_NOT_IMPLEMENTED);}; + virtual int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/){return (error = E_NOT_IMPLEMENTED);}; /// @copydoc KDL::SolverI::updateInternalDataStructures virtual void updateInternalDataStructures(); @@ -56,8 +50,8 @@ namespace KDL Jacobian jac; bool transpose,toggle; unsigned int m,n; - MatrixXd jac_eigen,U,V,B; - VectorXd S,tempi,tempj,UY,SUY,qdot_eigen,v_in_eigen; + Eigen::MatrixXd jac_eigen,U,V,B; + Eigen::VectorXd S,tempi,UY,SUY,qdot_eigen,v_in_eigen; }; } #endif diff --git a/orocos_kdl/src/chainiksolvervel_pinv_nso.cpp b/orocos_kdl/src/chainiksolvervel_pinv_nso.cpp index ceadbff0a..1b04795fb 100644 --- a/orocos_kdl/src/chainiksolvervel_pinv_nso.cpp +++ b/orocos_kdl/src/chainiksolvervel_pinv_nso.cpp @@ -29,12 +29,12 @@ namespace KDL jnt2jac(chain), nj(chain.getNrOfJoints()), jac(nj), - U(MatrixXd::Zero(6,nj)), - S(VectorXd::Zero(nj)), - Sinv(VectorXd::Zero(nj)), - V(MatrixXd::Zero(nj,nj)), - tmp(VectorXd::Zero(nj)), - tmp2(VectorXd::Zero(nj)), + U(Eigen::MatrixXd::Zero(6,nj)), + S(Eigen::VectorXd::Zero(nj)), + Sinv(Eigen::VectorXd::Zero(nj)), + V(Eigen::MatrixXd::Zero(nj,nj)), + tmp(Eigen::VectorXd::Zero(nj)), + tmp2(Eigen::VectorXd::Zero(nj)), eps(_eps), maxiter(_maxiter), svdResult(0), @@ -49,12 +49,12 @@ namespace KDL jnt2jac(chain), nj(chain.getNrOfJoints()), jac(nj), - U(MatrixXd::Zero(6,nj)), - S(VectorXd::Zero(nj)), - Sinv(VectorXd::Zero(nj)), - V(MatrixXd::Zero(nj,nj)), - tmp(VectorXd::Zero(nj)), - tmp2(VectorXd::Zero(nj)), + U(Eigen::MatrixXd::Zero(6,nj)), + S(Eigen::VectorXd::Zero(nj)), + Sinv(Eigen::VectorXd::Zero(nj)), + V(Eigen::MatrixXd::Zero(nj,nj)), + tmp(Eigen::VectorXd::Zero(nj)), + tmp2(Eigen::VectorXd::Zero(nj)), eps(_eps), maxiter(_maxiter), svdResult(0), @@ -66,14 +66,14 @@ namespace KDL jnt2jac.updateInternalDataStructures(); nj = chain.getNrOfJoints(); jac.resize(nj); - U.conservativeResizeLike(MatrixXd::Zero(6,nj)); - S.conservativeResizeLike(VectorXd::Zero(nj)); - Sinv.conservativeResizeLike(VectorXd::Zero(nj)); - V.conservativeResizeLike(MatrixXd::Zero(nj,nj)); - tmp.conservativeResizeLike(VectorXd::Zero(nj)); - tmp2.conservativeResizeLike(VectorXd::Zero(nj)); - opt_pos.data.conservativeResizeLike(VectorXd::Zero(nj)); - weights.data.conservativeResizeLike(VectorXd::Ones(nj)); + U.conservativeResizeLike(Eigen::MatrixXd::Zero(6,nj)); + S.conservativeResizeLike(Eigen::VectorXd::Zero(nj)); + Sinv.conservativeResizeLike(Eigen::VectorXd::Zero(nj)); + V.conservativeResizeLike(Eigen::MatrixXd::Zero(nj,nj)); + tmp.conservativeResizeLike(Eigen::VectorXd::Zero(nj)); + tmp2.conservativeResizeLike(Eigen::VectorXd::Zero(nj)); + opt_pos.data.conservativeResizeLike(Eigen::VectorXd::Zero(nj)); + weights.data.conservativeResizeLike(Eigen::VectorXd::Ones(nj)); } ChainIkSolverVel_pinv_nso::~ChainIkSolverVel_pinv_nso() diff --git a/orocos_kdl/src/chainiksolvervel_pinv_nso.hpp b/orocos_kdl/src/chainiksolvervel_pinv_nso.hpp index 0690bd4a2..97cd570ba 100644 --- a/orocos_kdl/src/chainiksolvervel_pinv_nso.hpp +++ b/orocos_kdl/src/chainiksolvervel_pinv_nso.hpp @@ -69,7 +69,7 @@ namespace KDL * not (yet) implemented. * */ - virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;}; + virtual int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/){return (error = E_NOT_IMPLEMENTED);}; /** * Request the joint weights for optimization criterion @@ -121,7 +121,7 @@ namespace KDL virtual int setOptPos(const JntArray &opt_pos); /** - *Set null psace velocity gain + *Set null space velocity gain * *@param alpha NUllspace velocity cgain * diff --git a/orocos_kdl/src/chainiksolvervel_wdls.cpp b/orocos_kdl/src/chainiksolvervel_wdls.cpp index f46eb1c12..a2f37aee8 100644 --- a/orocos_kdl/src/chainiksolvervel_wdls.cpp +++ b/orocos_kdl/src/chainiksolvervel_wdls.cpp @@ -30,19 +30,19 @@ namespace KDL jnt2jac(chain), nj(chain.getNrOfJoints()), jac(nj), - U(MatrixXd::Zero(6,nj)), - S(VectorXd::Zero(nj)), - V(MatrixXd::Zero(nj,nj)), + U(Eigen::MatrixXd::Zero(6,nj)), + S(Eigen::VectorXd::Zero(nj)), + V(Eigen::MatrixXd::Zero(nj,nj)), eps(_eps), maxiter(_maxiter), - tmp(VectorXd::Zero(nj)), - tmp_jac(MatrixXd::Zero(6,nj)), - tmp_jac_weight1(MatrixXd::Zero(6,nj)), - tmp_jac_weight2(MatrixXd::Zero(6,nj)), - tmp_ts(MatrixXd::Zero(6,6)), - tmp_js(MatrixXd::Zero(nj,nj)), - weight_ts(MatrixXd::Identity(6,6)), - weight_js(MatrixXd::Identity(nj,nj)), + tmp(Eigen::VectorXd::Zero(nj)), + tmp_jac(Eigen::MatrixXd::Zero(6,nj)), + tmp_jac_weight1(Eigen::MatrixXd::Zero(6,nj)), + tmp_jac_weight2(Eigen::MatrixXd::Zero(6,nj)), + tmp_ts(Eigen::MatrixXd::Zero(6,6)), + tmp_js(Eigen::MatrixXd::Zero(nj,nj)), + weight_ts(Eigen::MatrixXd::Identity(6,6)), + weight_js(Eigen::MatrixXd::Identity(nj,nj)), lambda(0.0), lambda_scaled(0.0), nrZeroSigmas(0), @@ -55,9 +55,9 @@ namespace KDL jnt2jac.updateInternalDataStructures(); nj = chain.getNrOfJoints(); jac.resize(nj); - MatrixXd z6nj = MatrixXd::Zero(6,nj); - VectorXd znj = VectorXd::Zero(nj); - MatrixXd znjnj = MatrixXd::Zero(nj,nj); + Eigen::MatrixXd z6nj = Eigen::MatrixXd::Zero(6,nj); + Eigen::VectorXd znj = Eigen::VectorXd::Zero(nj); + Eigen::MatrixXd znjnj = Eigen::MatrixXd::Zero(nj,nj); U.conservativeResizeLike(z6nj); S.conservativeResizeLike(znj); V.conservativeResizeLike(znjnj); @@ -66,14 +66,14 @@ namespace KDL tmp_jac_weight1.conservativeResizeLike(z6nj); tmp_jac_weight2.conservativeResizeLike(z6nj); tmp_js.conservativeResizeLike(znjnj); - weight_js.conservativeResizeLike(MatrixXd::Identity(nj,nj)); + weight_js.conservativeResizeLike(Eigen::MatrixXd::Identity(nj,nj)); } ChainIkSolverVel_wdls::~ChainIkSolverVel_wdls() { } - int ChainIkSolverVel_wdls::setWeightJS(const MatrixXd& Mq){ + int ChainIkSolverVel_wdls::setWeightJS(const Eigen::MatrixXd& Mq){ if(nj != chain.getNrOfJoints()) return (error = E_NOT_UP_TO_DATE); @@ -83,7 +83,7 @@ namespace KDL return (error = E_NOERROR); } - int ChainIkSolverVel_wdls::setWeightTS(const MatrixXd& Mx){ + int ChainIkSolverVel_wdls::setWeightTS(const Eigen::MatrixXd& Mx){ if (Mx.size() != weight_ts.size()) return (error = E_SIZE_MISMATCH); weight_ts = Mx; diff --git a/orocos_kdl/src/chainiksolvervel_wdls.hpp b/orocos_kdl/src/chainiksolvervel_wdls.hpp index 604048b87..dbba19b1a 100644 --- a/orocos_kdl/src/chainiksolvervel_wdls.hpp +++ b/orocos_kdl/src/chainiksolvervel_wdls.hpp @@ -102,7 +102,7 @@ namespace KDL * not (yet) implemented. * */ - virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;}; + virtual int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/){return -1;}; /** * Set the joint space weighting matrix diff --git a/orocos_kdl/src/chainjnttojacdotsolver.cpp b/orocos_kdl/src/chainjnttojacdotsolver.cpp index cb47d750d..9bb7e111b 100644 --- a/orocos_kdl/src/chainjnttojacdotsolver.cpp +++ b/orocos_kdl/src/chainjnttojacdotsolver.cpp @@ -23,6 +23,14 @@ namespace KDL { +const int ChainJntToJacDotSolver::E_JAC_DOT_FAILED; +const int ChainJntToJacDotSolver::E_JACSOLVER_FAILED; +const int ChainJntToJacDotSolver::E_FKSOLVERPOS_FAILED; + +const int ChainJntToJacDotSolver::HYBRID; +const int ChainJntToJacDotSolver::BODYFIXED; +const int ChainJntToJacDotSolver::INERTIAL; + ChainJntToJacDotSolver::ChainJntToJacDotSolver(const Chain& _chain): chain(_chain), locked_joints_(chain.getNrOfJoints(),false), @@ -85,7 +93,7 @@ int ChainJntToJacDotSolver::JntToJacDot(const JntArrayVel& q_in, Jacobian& jdot, if (representation_ == BODYFIXED) { // Ref Frame {ee}, Ref Point {ee} jac_.changeBase(F_bs_ee_.M.Inverse()); - } else if (representation_ == INTERTIAL) { + } else if (representation_ == INERTIAL) { // Ref Frame {bs}, Ref Point {bs} jac_.changeRefPoint(-F_bs_ee_.p); } else { @@ -98,7 +106,7 @@ int ChainJntToJacDotSolver::JntToJacDot(const JntArrayVel& q_in, Jacobian& jdot, for(unsigned int i=0;it_djdq_); @@ -133,8 +141,8 @@ const Twist& ChainJntToJacDotSolver::getPartialDerivative(const KDL::Jacobian& J } } -const Twist& ChainJntToJacDotSolver::getPartialDerivativeHybrid(const KDL::Jacobian& bs_J_ee, - const unsigned int& joint_idx, +const Twist& ChainJntToJacDotSolver::getPartialDerivativeHybrid(const KDL::Jacobian& bs_J_ee, + const unsigned int& joint_idx, const unsigned int& column_idx) { int j=joint_idx; @@ -164,8 +172,8 @@ const Twist& ChainJntToJacDotSolver::getPartialDerivativeHybrid(const KDL::Jacob return t_djdq_; } -const Twist& ChainJntToJacDotSolver::getPartialDerivativeBodyFixed(const Jacobian& ee_J_ee, - const unsigned int& joint_idx, +const Twist& ChainJntToJacDotSolver::getPartialDerivativeBodyFixed(const Jacobian& ee_J_ee, + const unsigned int& joint_idx, const unsigned int& column_idx) { int j=joint_idx; @@ -186,8 +194,8 @@ const Twist& ChainJntToJacDotSolver::getPartialDerivativeBodyFixed(const Jacobia return t_djdq_; } -const Twist& ChainJntToJacDotSolver::getPartialDerivativeInertial(const KDL::Jacobian& bs_J_bs, - const unsigned int& joint_idx, +const Twist& ChainJntToJacDotSolver::getPartialDerivativeInertial(const KDL::Jacobian& bs_J_bs, + const unsigned int& joint_idx, const unsigned int& column_idx) { int j=joint_idx; @@ -211,7 +219,7 @@ void ChainJntToJacDotSolver::setRepresentation(const int& representation) { if(representation == HYBRID || representation == BODYFIXED || - representation == INTERTIAL) + representation == INERTIAL) this->representation_ = representation; } diff --git a/orocos_kdl/src/chainjnttojacdotsolver.hpp b/orocos_kdl/src/chainjnttojacdotsolver.hpp index ecffa1355..b2d74d2fc 100644 --- a/orocos_kdl/src/chainjnttojacdotsolver.hpp +++ b/orocos_kdl/src/chainjnttojacdotsolver.hpp @@ -56,23 +56,23 @@ class ChainJntToJacDotSolver : public SolverI static const int HYBRID = 0; // Body-fixed representation ref Frame: end-effector, ref Point: end-effector static const int BODYFIXED = 1; - // Intertial representation ref Frame: base, ref Point: base - static const int INTERTIAL = 2; + // Inertial representation ref Frame: base, ref Point: base + static const int INERTIAL = 2; explicit ChainJntToJacDotSolver(const Chain& chain); virtual ~ChainJntToJacDotSolver(); /** * @brief Computes \f$ {}_{bs}\dot{J}^{ee}.\dot{q} \f$ - * + * * @param q_in Current joint positions and velocities * @param jac_dot_q_dot The twist representing Jdot*qdot * @param seg_nr The final segment to compute - * @return int 0 if no errors happened + * @return int 0 if no errors happened */ virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Twist& jac_dot_q_dot, int seg_nr = -1); /** - * @brief Computes \f$ {}_{bs}\dot{J}^{ee} \f$ - * + * @brief Computes \f$ {}_{bs}\dot{J}^{ee} \f$ + * * @param q_in Current joint positions and velocities * @param jdot The jacobian time derivative in the configured representation * (HYBRID, BODYFIXED or INERTIAL) @@ -81,34 +81,34 @@ class ChainJntToJacDotSolver : public SolverI */ virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Jacobian& jdot, int seg_nr = -1); int setLockedJoints(const std::vector& locked_joints); - + /** * @brief JntToJacDot() will compute in the Hybrid representation (ref Frame: base, ref Point: end-effector) - * - * + * + * * @return void */ void setHybridRepresentation(){setRepresentation(HYBRID);} /** * @brief JntToJacDot() will compute in the Body-fixed representation (ref Frame: end-effector, ref Point: end-effector) - * + * * @return void */ void setBodyFixedRepresentation(){setRepresentation(BODYFIXED);} /** * @brief JntToJacDot() will compute in the Inertial representation (ref Frame: base, ref Point: base) - * + * * @return void */ - void setInternialRepresentation(){setRepresentation(INTERTIAL);} + void setInertialRepresentation(){setRepresentation(INERTIAL);} /** * @brief Sets the internal variable for the representation (with a check on the value) - * - * @param representation The representation for Jdot : HYBRID,BODYFIXED or INTERTIAL + * + * @param representation The representation for Jdot : HYBRID,BODYFIXED or INERTIAL * @return void */ void setRepresentation(const int& representation); - + /// @copydoc KDL::SolverI::updateInternalDataStructures virtual void updateInternalDataStructures(); @@ -119,7 +119,7 @@ class ChainJntToJacDotSolver : public SolverI protected: /** * @brief Computes \f$ \frac{\partial {}_{bs}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$ - * + * * @param bs_J_ee The Jacobian expressed in the base frame with the end effector as the reference point (default in KDL Jacobian Solver) * @param joint_idx The index of the current joint (j in the formula) * @param column_idx The index of the current column (i in the formula) @@ -130,41 +130,41 @@ class ChainJntToJacDotSolver : public SolverI const unsigned int& column_idx); /** * @brief Computes \f$ \frac{\partial {}_{ee}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$ - * + * * @param bs_J_ee The Jacobian expressed in the end effector frame with the end effector as the reference point * @param joint_idx The indice of the current joint (j in the formula) * @param column_idx The indice of the current column (i in the formula) * @return Twist The twist representing dJi/dqj .qdotj - */ + */ const Twist& getPartialDerivativeBodyFixed(const Jacobian& ee_J_ee, const unsigned int& joint_idx, const unsigned int& column_idx); /** * @brief Computes \f$ \frac{\partial {}_{bs}J^{i,bs}}{\partial q^{j}}.\dot{q}^{j} \f$ - * + * * @param ee_J_ee The Jacobian expressed in the base frame with the base as the reference point * @param joint_idx The indice of the current joint (j in the formula) * @param column_idx The indice of the current column (i in the formula) * @return Twist The twist representing dJi/dqj .qdotj - */ + */ const Twist& getPartialDerivativeInertial(const Jacobian& bs_J_bs, const unsigned int& joint_idx, const unsigned int& column_idx); /** * @brief Computes \f$ \frac{\partial J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$ - * + * * @param bs_J_bs The Jacobian expressed in the base frame with the end effector as the reference point * @param joint_idx The indice of the current joint (j in the formula) * @param column_idx The indice of the current column (i in the formula) - * @param representation The representation (Hybrid,Body-fixed,Inertial) in which you want to get dJ/dqj .qdotj + * @param representation The representation (Hybrid,Body-fixed,Inertial) in which you want to get dJ/dqj .qdotj * @return Twist The twist representing dJi/dqj .qdotj - */ + */ const Twist& getPartialDerivative(const Jacobian& J, const unsigned int& joint_idx, const unsigned int& column_idx, const int& representation); private: - + const Chain& chain; std::vector locked_joints_; unsigned int nr_of_unlocked_joints_; diff --git a/orocos_kdl/src/chainjnttojacsolver.cpp b/orocos_kdl/src/chainjnttojacsolver.cpp index 54dd8915d..c24a9a312 100644 --- a/orocos_kdl/src/chainjnttojacsolver.cpp +++ b/orocos_kdl/src/chainjnttojacsolver.cpp @@ -1,8 +1,8 @@ -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2007 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -70,7 +70,7 @@ namespace KDL Frame total; for (unsigned int i=0;i struct std::hash +{ + std::size_t operator()(KDL::doubleAcc const& da) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, da.t); + KDL::hash_combine(seed, da.d); + KDL::hash_combine(seed, da.dd); + return seed; + } +}; + +template<> struct std::hash +{ + std::size_t operator()(KDL::VectorAcc const& va) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, va.p); + KDL::hash_combine(seed, va.v); + KDL::hash_combine(seed, va.dv); + return seed; + } +}; +template<> struct std::hash +{ + std::size_t operator()(KDL::RotationAcc const& ra) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, ra.R); + KDL::hash_combine(seed, ra.w); + KDL::hash_combine(seed, ra.dw); + return seed; + } +}; +template<> struct std::hash +{ + std::size_t operator()(KDL::FrameAcc const& fa) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, fa.M); + KDL::hash_combine(seed, fa.p); + return seed; + } +}; +template<> struct std::hash +{ + std::size_t operator()(KDL::TwistAcc const& ta) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, ta.vel); + KDL::hash_combine(seed, ta.rot); + return seed; + } +}; #endif diff --git a/orocos_kdl/src/frameacc.inl b/orocos_kdl/src/frameacc.inl index dfbdc620e..fa9c23ce4 100644 --- a/orocos_kdl/src/frameacc.inl +++ b/orocos_kdl/src/frameacc.inl @@ -126,9 +126,9 @@ void VectorAcc::ReverseSign() { dv.ReverseSign(); } -doubleAcc VectorAcc::Norm() { +doubleAcc VectorAcc::Norm(double eps) { doubleAcc res; - res.t = p.Norm(); + res.t = p.Norm(eps); res.d = dot(p,v)/res.t; res.dd = (dot(p,dv)+dot(v,v)-res.d*res.d)/res.t; return res; diff --git a/orocos_kdl/src/frames.cpp b/orocos_kdl/src/frames.cpp index 95fa1c9c5..4d478def4 100644 --- a/orocos_kdl/src/frames.cpp +++ b/orocos_kdl/src/frames.cpp @@ -26,9 +26,8 @@ ***************************************************************************/ #include "frames.hpp" +#include "utilities/utility.h" -#define _USE_MATH_DEFINES // For MSVC -#include #include namespace KDL { @@ -85,13 +84,13 @@ namespace KDL { ); } - double Vector2::Norm() const + double Vector2::Norm(double eps) const { double tmp1 = fabs(data[0]); double tmp2 = fabs(data[1]); - if (tmp1 == 0.0 && tmp2 == 0.0) - return 0.0; + if (tmp1 < eps && tmp2 < eps) + return 0; if (tmp1 > tmp2) { return tmp1*sqrt(1+sqr(data[1]/data[0])); @@ -100,13 +99,13 @@ namespace KDL { } } // makes v a unitvector and returns the norm of v. - // if v is smaller than eps, Vector(1,0,0) is returned with norm 0. + // if v is smaller than eps, Vector(1,0) is returned with norm 0. // if this is not good, check the return value of this method. double Vector2::Normalize(double eps) { double v = this->Norm(); if (v < eps) { *this = Vector2(1,0); - return v; + return 0; } else { *this = (*this)/v; return v; @@ -115,7 +114,7 @@ namespace KDL { // do some effort not to lose precision - double Vector::Norm() const + double Vector::Norm(double eps) const { double tmp1; double tmp2; @@ -124,7 +123,7 @@ namespace KDL { if (tmp1 >= tmp2) { tmp2=fabs(data[2]); if (tmp1 >= tmp2) { - if (tmp1 == 0) { + if (tmp1 < eps) { // only to everything exactly zero case, all other are handled correctly return 0; } @@ -149,7 +148,7 @@ namespace KDL { double v = this->Norm(); if (v < eps) { *this = Vector(1,0,0); - return v; + return 0; } else { *this = (*this)/v; return v; @@ -251,7 +250,7 @@ void Rotation::GetRPY(double& roll,double& pitch,double& yaw) const { double epsilon=1E-12; pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) ) ); - if ( fabs(pitch) > (M_PI/2.0-epsilon) ) { + if ( fabs(pitch) > (PI_2-epsilon) ) { yaw = atan2( -data[1], data[4]); roll = 0.0 ; } else { @@ -384,7 +383,7 @@ double Rotation::GetRotAngle(Vector& axis,double eps) const { } // otherwise this singularity is angle = 180 - angle = M_PI; + angle = PI; double xx = (data[0] + 1) / 2; double yy = (data[4] + 1) / 2; double zz = (data[8] + 1) / 2; @@ -392,8 +391,6 @@ double Rotation::GetRotAngle(Vector& axis,double eps) const { double xz = (data[2] + data[6]) / 4; double yz = (data[5] + data[7]) / 4; - double half_sqrt_2 = 0.5 * sqrt(2.0); - if ((xx > yy) && (xx > zz)) { // data[0] is the largest diagonal term @@ -419,15 +416,13 @@ double Rotation::GetRotAngle(Vector& axis,double eps) const { return angle; // return 180 deg rotation } - // If the matrix is slightly non-orthogonal, `f` may be out of the (-1, +1) range. - // Therefore, clamp it between those values to avoid NaNs. double f = (data[0] + data[4] + data[8] - 1) / 2; - angle = acos(std::max(-1.0, std::min(1.0, f))); x = (data[7] - data[5]); y = (data[2] - data[6]); z = (data[3] - data[1]); axis = KDL::Vector(x, y, z); + angle = atan2(axis.Norm()/2,f); axis.Normalize(); return angle; } diff --git a/orocos_kdl/src/frames.hpp b/orocos_kdl/src/frames.hpp index 1764c1487..d816f0da2 100644 --- a/orocos_kdl/src/frames.hpp +++ b/orocos_kdl/src/frames.hpp @@ -127,13 +127,15 @@ #include "utilities/kdl-config.h" #include "utilities/utility.h" +#include "utilities/hash_combine.h" + +#include ///////////////////////////////////////////////////////////// namespace KDL { - class Vector; class Rotation; class Frame; @@ -237,7 +239,7 @@ class Vector double Normalize(double eps=epsilon); //! @return the norm of the vector - double Norm() const; + double Norm(double eps=epsilon) const; @@ -310,8 +312,8 @@ class Rotation double Xy,double Yy,double Zy, double Xz,double Yz,double Zz); inline Rotation(const Vector& x,const Vector& y,const Vector& z); - // default copy constructor is sufficient + inline Rotation(const Rotation& arg); inline Rotation& operator=(const Rotation& arg); @@ -711,6 +713,7 @@ class Frame { inline friend bool operator!=(const Frame& a,const Frame& b); }; + /** * \brief represents both translational and rotational velocities. * @@ -788,9 +791,9 @@ class Twist { // = Friends friend class Rotation; friend class Frame; - }; + /** * \brief represents both translational and rotational acceleration. * @@ -950,8 +953,6 @@ class Wrench friend class Rotation; friend class Frame; - - }; @@ -1015,7 +1016,7 @@ class Vector2 double Normalize(double eps=epsilon); //! @return the norm of the vector - double Norm() const; + double Norm(double eps=epsilon) const; //! projects v in its XY plane, and sets *this to these values inline void Set3DXY(const Vector& v); @@ -1060,6 +1061,8 @@ class Rotation2 Rotation2(double ca,double sa):s(sa),c(ca){} + Rotation2(const Rotation2& arg); + inline Rotation2& operator=(const Rotation2& arg); inline Vector2 operator*(const Vector2& v) const; //! Access to elements 0..1,0..1, bounds are checked when NDEBUG is not set @@ -1089,6 +1092,7 @@ class Rotation2 inline friend bool Equal(const Rotation2& a,const Rotation2& b,double eps); }; + //! A 2D frame class, for further documentation see the Frames class //! for methods with unchanged semantics. class Frame2 @@ -1250,20 +1254,101 @@ IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1); IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1); #ifdef KDL_INLINE -// #include "vector.inl" -// #include "wrench.inl" - //#include "rotation.inl" - //#include "frame.inl" - //#include "twist.inl" - //#include "vector2.inl" - //#include "rotation2.inl" - //#include "frame2.inl" #include "frames.inl" #endif +} + +template<> struct std::hash +{ + std::size_t operator()(KDL::Vector const& v) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, v.x()); + KDL::hash_combine(seed, v.y()); + KDL::hash_combine(seed, v.z()); + return seed; + } +}; +template<> struct std::hash +{ + std::size_t operator()(KDL::Rotation const& r) const noexcept + { + size_t seed = 0; + double x, y, z, w; + r.GetQuaternion(x, y, z, w); + KDL::hash_combine(seed, x); + KDL::hash_combine(seed, y); + KDL::hash_combine(seed, z); + KDL::hash_combine(seed, w); + return seed; + } +}; -} +template<> struct std::hash +{ + std::size_t operator()(KDL::Frame const& f) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, f.p); + KDL::hash_combine(seed, f.M); + return seed; + } +}; +template<> struct std::hash +{ + std::size_t operator()(KDL::Wrench const& w) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, w.force); + KDL::hash_combine(seed, w.torque); + return seed; + } +}; + +template<> struct std::hash +{ + std::size_t operator()(KDL::Twist const& t) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, t.vel); + KDL::hash_combine(seed, t.rot); + return seed; + } +}; + +template<> struct std::hash +{ + std::size_t operator()(KDL::Vector2 const& v) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, v.x()); + KDL::hash_combine(seed, v.y()); + return seed; + } +}; + +template<> struct std::hash +{ + std::size_t operator()(KDL::Rotation2 const& r) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, r.GetRot()); + return seed; + } +}; + +template<> struct std::hash +{ + std::size_t operator()(KDL::Frame2 const& f) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, f.p); + KDL::hash_combine(seed, f.M); + return seed; + } +}; #endif diff --git a/orocos_kdl/src/frames.inl b/orocos_kdl/src/frames.inl index ac62a997a..8349ff108 100644 --- a/orocos_kdl/src/frames.inl +++ b/orocos_kdl/src/frames.inl @@ -512,6 +512,11 @@ Rotation::Rotation(const Vector& x,const Vector& y,const Vector& z) data[2] = z.data[0];data[5] = z.data[1];data[8] = z.data[2]; } +Rotation::Rotation(const Rotation& arg) { + int count=9; + while (count--) data[count] = arg.data[count]; +} + Rotation& Rotation::operator=(const Rotation& arg) { int count=9; while (count--) data[count] = arg.data[count]; @@ -839,7 +844,9 @@ IMETHOD void Vector2::Set3DPlane(const Frame& F_someframe_XY,const Vector& v_som data[1]=tmp(1); } - +IMETHOD Rotation2::Rotation2(const Rotation2& arg) { + c=arg.c;s=arg.s; +} IMETHOD Rotation2& Rotation2::operator=(const Rotation2& arg) { c=arg.c;s=arg.s; @@ -1151,7 +1158,7 @@ IMETHOD Vector addDelta(const Vector& a,const Vector&da,double dt) { } IMETHOD Rotation addDelta(const Rotation& a,const Vector&da,double dt) { - return a*Rot(a.Inverse(da)*dt); + return Rot(da*dt)*a; } IMETHOD Frame addDelta(const Frame& a,const Twist& da,double dt) { return Frame( diff --git a/orocos_kdl/src/framevel.hpp b/orocos_kdl/src/framevel.hpp index 243898a60..b84e69c1f 100644 --- a/orocos_kdl/src/framevel.hpp +++ b/orocos_kdl/src/framevel.hpp @@ -30,7 +30,6 @@ #include "frames.hpp" - namespace KDL { typedef Rall1d doubleVel; @@ -81,6 +80,7 @@ IMETHOD bool Equal(const TwistVel& a,const TwistVel& b,double eps=epsilon); IMETHOD bool Equal(const Twist& a,const TwistVel& b,double eps=epsilon); IMETHOD bool Equal(const TwistVel& a,const Twist& b,double eps=epsilon); + class VectorVel // = TITLE // An VectorVel is a Vector and its first derivative @@ -104,7 +104,7 @@ class VectorVel IMETHOD VectorVel& operator -= (const VectorVel& arg); IMETHOD static VectorVel Zero(); IMETHOD void ReverseSign(); - IMETHOD doubleVel Norm() const; + IMETHOD doubleVel Norm(double eps=epsilon) const; IMETHOD friend VectorVel operator + (const VectorVel& r1,const VectorVel& r2); IMETHOD friend VectorVel operator - (const VectorVel& r1,const VectorVel& r2); IMETHOD friend VectorVel operator + (const Vector& r1,const VectorVel& r2); @@ -128,6 +128,14 @@ class VectorVel IMETHOD friend bool Equal(const VectorVel& r1,const VectorVel& r2,double eps); IMETHOD friend bool Equal(const Vector& r1,const VectorVel& r2,double eps); IMETHOD friend bool Equal(const VectorVel& r1,const Vector& r2,double eps); + + IMETHOD friend bool operator==(const VectorVel& r1,const VectorVel& r2); + IMETHOD friend bool operator!=(const VectorVel& r1,const VectorVel& r2); + IMETHOD friend bool operator==(const Vector& r1,const VectorVel& r2); + IMETHOD friend bool operator!=(const Vector& r1,const VectorVel& r2); + IMETHOD friend bool operator==(const VectorVel& r1,const Vector& r2); + IMETHOD friend bool operator!=(const VectorVel& r1,const Vector& r2); + IMETHOD friend VectorVel operator - (const VectorVel& r); IMETHOD friend doubleVel dot(const VectorVel& lhs,const VectorVel& rhs); IMETHOD friend doubleVel dot(const VectorVel& lhs,const Vector& rhs); @@ -135,7 +143,6 @@ class VectorVel }; - class RotationVel // = TITLE // An RotationVel is a Rotation and its first derivative, a rotation vector @@ -185,6 +192,13 @@ class RotationVel IMETHOD friend bool Equal(const Rotation& r1,const RotationVel& r2,double eps); IMETHOD friend bool Equal(const RotationVel& r1,const Rotation& r2,double eps); + IMETHOD friend bool operator==(const RotationVel& r1,const RotationVel& r2); + IMETHOD friend bool operator!=(const RotationVel& r1,const RotationVel& r2); + IMETHOD friend bool operator==(const Rotation& r1,const RotationVel& r2); + IMETHOD friend bool operator!=(const Rotation& r1,const RotationVel& r2); + IMETHOD friend bool operator==(const RotationVel& r1,const Rotation& r2); + IMETHOD friend bool operator!=(const RotationVel& r1,const Rotation& r2); + IMETHOD TwistVel Inverse(const TwistVel& arg) const; IMETHOD TwistVel Inverse(const Twist& arg) const; IMETHOD TwistVel operator * (const TwistVel& arg) const; @@ -192,8 +206,6 @@ class RotationVel }; - - class FrameVel // = TITLE // An FrameVel is a Frame and its first derivative, a Twist vector @@ -239,6 +251,13 @@ class FrameVel IMETHOD friend bool Equal(const Frame& r1,const FrameVel& r2,double eps); IMETHOD friend bool Equal(const FrameVel& r1,const Frame& r2,double eps); + IMETHOD friend bool operator==(const FrameVel& a,const FrameVel& b); + IMETHOD friend bool operator!=(const FrameVel& a,const FrameVel& b); + IMETHOD friend bool operator==(const Frame& a,const FrameVel& b); + IMETHOD friend bool operator!=(const Frame& a,const FrameVel& b); + IMETHOD friend bool operator==(const FrameVel& a,const Frame& b); + IMETHOD friend bool operator!=(const FrameVel& a,const Frame& b); + IMETHOD TwistVel Inverse(const TwistVel& arg) const; IMETHOD TwistVel Inverse(const Twist& arg) const; IMETHOD TwistVel operator * (const TwistVel& arg) const; @@ -246,9 +265,6 @@ class FrameVel }; - - - //very similar to Wrench class. class TwistVel // = TITLE @@ -311,6 +327,13 @@ class TwistVel IMETHOD friend bool Equal(const Twist& a,const TwistVel& b,double eps); IMETHOD friend bool Equal(const TwistVel& a,const Twist& b,double eps); + IMETHOD friend bool operator==(const TwistVel& a,const TwistVel& b); + IMETHOD friend bool operator!=(const TwistVel& a,const TwistVel& b); + IMETHOD friend bool operator==(const Twist& a,const TwistVel& b); + IMETHOD friend bool operator!=(const Twist& a,const TwistVel& b); + IMETHOD friend bool operator==(const TwistVel& a,const Twist& b); + IMETHOD friend bool operator!=(const TwistVel& a,const Twist& b); + // = Conversion to other entities IMETHOD Twist GetTwist() const; IMETHOD Twist GetTwistDot() const; @@ -387,10 +410,61 @@ IMETHOD void posrandom(FrameVel& F) { #include "framevel.inl" #endif -} // namespace +} // namespace KDL -#endif +template<> struct std::hash +{ + std::size_t operator()(KDL::doubleVel const& dv) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, dv.value()); + KDL::hash_combine(seed, dv.deriv()); + return seed; + } +}; +template<> struct std::hash +{ + std::size_t operator()(KDL::VectorVel const& vv) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, vv.p); + KDL::hash_combine(seed, vv.v); + return seed; + } +}; +template<> struct std::hash +{ + std::size_t operator()(KDL::RotationVel const& rv) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, rv.R); + KDL::hash_combine(seed, rv.w); + return seed; + } +}; +template<> struct std::hash +{ + std::size_t operator()(KDL::FrameVel const& fv) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, fv.M); + KDL::hash_combine(seed, fv.p); + return seed; + } +}; +template<> struct std::hash +{ + std::size_t operator()(KDL::TwistVel const& tv) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, tv.vel); + KDL::hash_combine(seed, tv.rot); + return seed; + } +}; + +#endif diff --git a/orocos_kdl/src/framevel.inl b/orocos_kdl/src/framevel.inl index 61a43fdc2..05aceb0af 100644 --- a/orocos_kdl/src/framevel.inl +++ b/orocos_kdl/src/framevel.inl @@ -81,6 +81,41 @@ bool Equal(const Frame& r1,const FrameVel& r2,double eps) { bool Equal(const FrameVel& r1,const Frame& r2,double eps) { return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); } +bool operator==(const FrameVel& r1,const FrameVel& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1.p == r2.p && + r1.M == r2.M ); +#endif +} +bool operator!=(const FrameVel& r1,const FrameVel& r2) { + return !operator==(r1,r2); +} +bool operator==(const Frame& r1,const FrameVel& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1.p == r2.p && + r1.M == r2.M ); +#endif +} +bool operator!=(const Frame& r1,const FrameVel& r2) { + return !operator==(r1,r2); +} +bool operator==(const FrameVel& r1,const Frame& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1.p == r2.p && + r1.M == r2.M ); +#endif +} +bool operator!=(const FrameVel& r1,const Frame& r2) { + return !operator==(r1,r2); +} + + Frame FrameVel::GetFrame() const { return Frame(M.R,p.p); @@ -315,9 +350,11 @@ void VectorVel::ReverseSign() { p.ReverseSign(); v.ReverseSign(); } -doubleVel VectorVel::Norm() const { - double n = p.Norm(); - return doubleVel(n,dot(p,v)/n); +doubleVel VectorVel::Norm(double eps) const { + double n = p.Norm(eps); + if (n < eps) // Setting norm of p and v to 0 in case norm of p is smaller than eps + return doubleVel(0, 0); + return doubleVel(n, dot(p,v)/n); } bool Equal(const VectorVel& r1,const VectorVel& r2,double eps) { @@ -329,6 +366,40 @@ bool Equal(const Vector& r1,const VectorVel& r2,double eps) { bool Equal(const VectorVel& r1,const Vector& r2,double eps) { return (Equal(r1.p,r2,eps) && Equal(r1.v,Vector::Zero(),eps)); } +bool operator==(const VectorVel& r1,const VectorVel& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1.p == r2.p && + r1.v == r2.v ); +#endif +} +bool operator!=(const VectorVel& r1,const VectorVel& r2) { + return !operator==(r1,r2); +} +bool operator==(const Vector& r1,const VectorVel& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1 == r2.p && + Vector::Zero() == r2.v); +#endif +} +bool operator!=(const Vector& r1,const VectorVel& r2) { + return !operator==(r1,r2); +} +bool operator==(const VectorVel& r1,const Vector& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1.p == r2 && + r1.v == Vector::Zero() ); +#endif +} +bool operator!=(const VectorVel& r1,const Vector& r2) { + return !operator==(r1,r2); +} + bool Equal(const RotationVel& r1,const RotationVel& r2,double eps) { return (Equal(r1.w,r2.w,eps) && Equal(r1.R,r2.R,eps)); @@ -339,6 +410,41 @@ bool Equal(const Rotation& r1,const RotationVel& r2,double eps) { bool Equal(const RotationVel& r1,const Rotation& r2,double eps) { return (Equal(r1.w,Vector::Zero(),eps) && Equal(r1.R,r2,eps)); } +bool operator==(const RotationVel& r1,const RotationVel& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1.w == r2.w && + r1.R == r2.R ); +#endif +} +bool operator!=(const RotationVel& r1,const RotationVel& r2) { + return !operator==(r1,r2); +} +bool operator==(const Rotation& r1,const RotationVel& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (Vector::Zero() == r2.w && + r1 == r2.R); +#endif +} +bool operator!=(const Rotation& r1,const RotationVel& r2) { + return !operator==(r1,r2); +} +bool operator==(const RotationVel& r1,const Rotation& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1.w == Vector::Zero() && + r1.R == r2); +#endif +} +bool operator!=(const RotationVel& r1,const Rotation& r2) { + return !operator==(r1,r2); +} + + bool Equal(const TwistVel& a,const TwistVel& b,double eps) { return (Equal(a.rot,b.rot,eps)&& Equal(a.vel,b.vel,eps) ); @@ -351,7 +457,39 @@ bool Equal(const TwistVel& a,const Twist& b,double eps) { return (Equal(a.rot,b.rot,eps)&& Equal(a.vel,b.vel,eps) ); } - +bool operator==(const TwistVel& a,const TwistVel& b) { +#ifdef KDL_USE_EQUAL + return Equal(a, b); +#else + return (a.rot == b.rot && + a.vel == b.vel ); +#endif +} +bool operator!=(const TwistVel& a,const TwistVel& b) { + return !operator==(a,b); +} +bool operator==(const Twist& a,const TwistVel& b) { +#ifdef KDL_USE_EQUAL + return Equal(a, b); +#else + return (a.rot == b.rot && + a.vel == b.vel ); +#endif +} +bool operator!=(const Twist& r1,const TwistVel& r2) { + return !operator==(r1,r2); +} +bool operator==(const TwistVel& r1,const Twist& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (a.rot == b.rot && + a.vel == b.vel ); +#endif +} +bool operator!=(const TwistVel& r1,const Twist& r2) { + return !operator==(r1,r2); +} IMETHOD doubleVel dot(const VectorVel& lhs,const VectorVel& rhs) { @@ -364,17 +502,6 @@ IMETHOD doubleVel dot(const Vector& lhs,const VectorVel& rhs) { return doubleVel(dot(lhs,rhs.p),dot(lhs,rhs.v)); } - - - - - - - - - - - TwistVel TwistVel::Zero() { return TwistVel(VectorVel::Zero(),VectorVel::Zero()); diff --git a/orocos_kdl/src/framevel_io.hpp b/orocos_kdl/src/framevel_io.hpp index 2124c13c4..a3dcc0743 100644 --- a/orocos_kdl/src/framevel_io.hpp +++ b/orocos_kdl/src/framevel_io.hpp @@ -21,7 +21,6 @@ #include "utilities/utility_io.h" #include "utilities/rall1d_io.h" -#include "framevel_io.hpp" #include "frames_io.hpp" namespace KDL { diff --git a/orocos_kdl/src/jacobian.cpp b/orocos_kdl/src/jacobian.cpp index 839ad0011..21d3d649f 100644 --- a/orocos_kdl/src/jacobian.cpp +++ b/orocos_kdl/src/jacobian.cpp @@ -1,8 +1,8 @@ -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2007 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -23,8 +23,6 @@ namespace KDL { - using namespace Eigen; - Jacobian::Jacobian() { } @@ -33,15 +31,16 @@ namespace KDL Jacobian::Jacobian(unsigned int nr_of_columns): data(6,nr_of_columns) { + data.setZero(); } - + Jacobian::Jacobian(const Jacobian& arg): data(arg.data) { } Jacobian& Jacobian::operator = (const Jacobian& arg) - { + { this->data=arg.data; return *this; } @@ -49,7 +48,7 @@ namespace KDL Jacobian::~Jacobian() { - + } void Jacobian::resize(unsigned int new_nr_of_columns) @@ -69,12 +68,12 @@ namespace KDL unsigned int Jacobian::rows()const { - return data.rows(); + return static_cast(data.rows()); } unsigned int Jacobian::columns()const { - return data.cols(); + return static_cast(data.cols()); } void SetToZero(Jacobian& jac) @@ -95,7 +94,7 @@ namespace KDL dest.setColumn(i,src1.getColumn(i).RefPoint(base_AB)); return true; } - + void Jacobian::changeBase(const Rotation& rot){ for(unsigned int i=0;isetColumn(i,rot*this->getColumn(i));; @@ -114,7 +113,7 @@ namespace KDL for(unsigned int i=0;isetColumn(i,frame*this->getColumn(i)); } - + bool changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest) { if(src1.columns()!=dest.columns()) @@ -128,27 +127,27 @@ namespace KDL { return Equal((*this),arg); } - + bool Jacobian::operator!=(const Jacobian& arg)const { return !Equal((*this),arg); } - + bool Equal(const Jacobian& a,const Jacobian& b,double eps) { if(a.rows()==b.rows()&&a.columns()==b.columns()){ - return a.data.isApprox(b.data,eps); + return (a.data-b.data).isZero(eps); }else return false; } - + Twist Jacobian::getColumn(unsigned int i) const{ return Twist(Vector(data(0,i),data(1,i),data(2,i)),Vector(data(3,i),data(4,i),data(5,i))); } - + void Jacobian::setColumn(unsigned int i,const Twist& t){ - data.col(i).head<3>()=Eigen::Map(t.vel.data); - data.col(i).tail<3>()=Eigen::Map(t.rot.data); + data.col(i).head<3>()=Eigen::Map(t.vel.data); + data.col(i).tail<3>()=Eigen::Map(t.rot.data); } } diff --git a/orocos_kdl/src/jntarray.cpp b/orocos_kdl/src/jntarray.cpp index 2db5bd9e1..32c875d0b 100644 --- a/orocos_kdl/src/jntarray.cpp +++ b/orocos_kdl/src/jntarray.cpp @@ -23,8 +23,6 @@ namespace KDL { - using namespace Eigen; - JntArray::JntArray() { } @@ -54,7 +52,7 @@ namespace KDL void JntArray::resize(unsigned int newSize) { - data.conservativeResizeLike(VectorXd::Zero(newSize)); + data.conservativeResizeLike(Eigen::VectorXd::Zero(newSize)); } double JntArray::operator()(unsigned int i,unsigned int j)const @@ -71,12 +69,12 @@ namespace KDL unsigned int JntArray::rows()const { - return data.rows(); + return static_cast(data.rows()); } unsigned int JntArray::columns()const { - return data.cols(); + return static_cast(data.cols()); } void Add(const JntArray& src1,const JntArray& src2,JntArray& dest) @@ -114,11 +112,11 @@ namespace KDL { if(src1.rows()!=src2.rows()) return false; - return src1.data.isApprox(src2.data,eps); + return (src1.data-src2.data).isZero(eps); } - bool operator==(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);}; - //bool operator!=(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);}; + bool operator==(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);} + //bool operator!=(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);} } diff --git a/orocos_kdl/src/jntspaceinertiamatrix.cpp b/orocos_kdl/src/jntspaceinertiamatrix.cpp index dea5cb646..fb8a4fed4 100644 --- a/orocos_kdl/src/jntspaceinertiamatrix.cpp +++ b/orocos_kdl/src/jntspaceinertiamatrix.cpp @@ -1,8 +1,8 @@ -// Copyright (C) 2009 Dominick Vanthienen +// Copyright (C) 2009 Dominick Vanthienen // Version: 1.0 -// Author: Dominick Vanthienen -// Maintainer: Dominick Vanthienen +// Author: Dominick Vanthienen +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -23,8 +23,6 @@ namespace KDL { - using namespace Eigen; - JntSpaceInertiaMatrix::JntSpaceInertiaMatrix() { } @@ -69,12 +67,12 @@ namespace KDL unsigned int JntSpaceInertiaMatrix::rows()const { - return data.rows(); + return static_cast(data.rows()); } unsigned int JntSpaceInertiaMatrix::columns()const { - return data.cols(); + return static_cast(data.cols()); } @@ -112,12 +110,11 @@ namespace KDL { if(src1.rows()!=src2.rows()||src1.columns()!=src2.columns()) return false; - return src1.data.isApprox(src2.data,eps); + return (src1.data-src2.data).isZero(eps); } - bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);}; - //bool operator!=(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);}; - + bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);} + //bool operator!=(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);} } diff --git a/orocos_kdl/src/jntspaceinertiamatrix.hpp b/orocos_kdl/src/jntspaceinertiamatrix.hpp index ae4f01298..fc386cf35 100644 --- a/orocos_kdl/src/jntspaceinertiamatrix.hpp +++ b/orocos_kdl/src/jntspaceinertiamatrix.hpp @@ -1,8 +1,8 @@ -// Copyright (C) 2009 Dominick Vanthienen +// Copyright (C) 2009 Dominick Vanthienen // Version: 1.0 -// Author: Dominick Vanthienen -// Maintainer: Dominick Vanthienen +// Author: Dominick Vanthienen +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or diff --git a/orocos_kdl/src/joint.cpp b/orocos_kdl/src/joint.cpp index e487334b8..e27bf2516 100644 --- a/orocos_kdl/src/joint.cpp +++ b/orocos_kdl/src/joint.cpp @@ -1,8 +1,8 @@ -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2007 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -28,8 +28,8 @@ namespace KDL { const double& _inertia, const double& _damping, const double& _stiffness): name(_name),type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness) { - if (type == RotAxis || type == TransAxis) throw joint_type_ex; - q_previous = 0; + if (type == RotAxis || type == TransAxis) + throw joint_type_ex; } // constructor for joint along x,y or z axis, at origin of reference frame @@ -37,36 +37,28 @@ namespace KDL { const double& _inertia, const double& _damping, const double& _stiffness): name("NoName"),type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness) { - if (type == RotAxis || type == TransAxis) throw joint_type_ex; - q_previous = 0; + if (type == RotAxis || type == TransAxis) + throw joint_type_ex; } // constructor for joint along arbitrary axis, at arbitrary origin - Joint::Joint(const std::string& _name, const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, + Joint::Joint(const std::string& _name, const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness): name(_name), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness) , axis(_axis / _axis.Norm()), origin(_origin) { - if (type != RotAxis && type != TransAxis) throw joint_type_ex; - - // initialize - joint_pose.p = origin; - joint_pose.M = Rotation::Rot2(axis, offset); - q_previous = 0; + if (type != RotAxis && type != TransAxis) + throw joint_type_ex; } // constructor for joint along arbitrary axis, at arbitrary origin - Joint::Joint(const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, + Joint::Joint(const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness): name("NoName"), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness), axis(_axis / _axis.Norm()),origin(_origin) { - if (type != RotAxis && type != TransAxis) throw joint_type_ex; - - // initialize - joint_pose.p = origin; - joint_pose.M = Rotation::Rot2(axis, offset); - q_previous = 0; + if (type != RotAxis && type != TransAxis) + throw joint_type_ex; } Joint::~Joint() @@ -77,12 +69,7 @@ namespace KDL { { switch(type){ case RotAxis: - // calculate the rotation matrix around the vector "axis" - if (q != q_previous){ - q_previous = q; - joint_pose.M = Rotation::Rot2(axis, scale*q+offset); - } - return joint_pose; + return Frame(Rotation::Rot2(axis, scale*q+offset), origin); case RotX: return Frame(Rotation::RotX(scale*q+offset)); case RotY: @@ -97,7 +84,7 @@ namespace KDL { return Frame(Vector(0.0,scale*q+offset,0.0)); case TransZ: return Frame(Vector(0.0,0.0,scale*q+offset)); - case None: + case Fixed: return Frame::Identity(); } return Frame::Identity(); @@ -122,7 +109,7 @@ namespace KDL { return Twist(Vector(0.0,scale*qdot,0.0),Vector(0.0,0.0,0.0)); case TransZ: return Twist(Vector(0.0,0.0,scale*qdot),Vector(0.0,0.0,0.0)); - case None: + case Fixed: return Twist::Zero(); } return Twist::Zero(); @@ -148,7 +135,7 @@ namespace KDL { return Vector(0.,1.,0.); case TransZ: return Vector(0.,0.,1.); - case None: + case Fixed: return Vector::Zero(); } return Vector::Zero(); diff --git a/orocos_kdl/src/joint.hpp b/orocos_kdl/src/joint.hpp index a188aff95..c7fccd09b 100644 --- a/orocos_kdl/src/joint.hpp +++ b/orocos_kdl/src/joint.hpp @@ -1,8 +1,8 @@ -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2007 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -44,12 +44,12 @@ namespace KDL { */ class Joint { public: - typedef enum { RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,None} JointType; + typedef enum { RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,Fixed,None=Fixed} JointType; /** * Constructor of a joint. * * @param name of the joint - * @param type type of the joint, default: Joint::None + * @param type type of the joint, default: Joint::Fixed * @param scale scale between joint input and actual geometric * movement, default: 1 * @param offset offset between joint input and actual @@ -59,12 +59,12 @@ namespace KDL { * @param stiffness 1D stiffness along the joint axis, * default: 0 */ - explicit Joint(const std::string& name, const JointType& type=None,const double& scale=1,const double& offset=0, + explicit Joint(const std::string& name, const JointType& type=Fixed,const double& scale=1,const double& offset=0, const double& inertia=0,const double& damping=0,const double& stiffness=0); /** * Constructor of a joint. * - * @param type type of the joint, default: Joint::None + * @param type type of the joint, default: Joint::Fixed * @param scale scale between joint input and actual geometric * movement, default: 1 * @param offset offset between joint input and actual @@ -74,7 +74,7 @@ namespace KDL { * @param stiffness 1D stiffness along the joint axis, * default: 0 */ - explicit Joint(const JointType& type=None,const double& scale=1,const double& offset=0, + explicit Joint(const JointType& type=Fixed,const double& scale=1,const double& offset=0, const double& inertia=0,const double& damping=0,const double& stiffness=0); /** * Constructor of a joint. @@ -128,16 +128,16 @@ namespace KDL { */ Twist twist(const double& qdot)const; - /** - * Request the Vector corresponding to the axis of a revolute joint. - * - * @return Vector. e.g (1,0,0) for RotX etc. + /** + * Request the Vector corresponding to the axis of a revolute joint. + * + * @return Vector. e.g (1,0,0) for RotX etc. */ Vector JointAxis() const; - /** - * Request the Vector corresponding to the origin of a revolute joint. - * + /** + * Request the Vector corresponding to the origin of a revolute joint. + * * @return Vector */ Vector JointOrigin() const; @@ -160,8 +160,8 @@ namespace KDL { { return type; }; - - /** + + /** * Request the stringified type of the joint. * * @return const string @@ -185,13 +185,83 @@ namespace KDL { return "TransY"; case TransZ: return "TransZ"; - case None: - return "None"; + case Fixed: + return "Fixed"; default: - return "None"; + return "Fixed"; } }; + /** + * Request the scale of the joint. + * + * @return const reference to the scale of the joint + */ + const double& getScale() const + { + return scale; + } + + /** + * Request the offset of the joint. + * + * @return const reference to the offset of the joint + */ + const double& getOffset() const + { + return offset; + } + + /** + * Request the inertia of the joint. + * + * @return const reference to the inertia of the joint + */ + const double& getInertia() const + { + return inertia; + }; + + /** + * Request the damping of the joint. + * + * @return const reference to the damping of the joint + */ + const double& getDamping() const + { + return damping; + }; + + /** + * Request the stiffness of the joint. + * + * @return const reference to the stiffness of the joint + */ + const double& getStiffness() const + { + return stiffness; + }; + + /** + * Request the axis of the joint. + * + * @return const reference to the axis of the joint + */ + const Vector& getAxis() const + { + return axis; + } + + /** + * Request the origin of the joint. + * + * @return const reference to the origin of the joint + */ + const Vector& getOrigin() const + { + return origin; + } + virtual ~Joint(); private: @@ -205,11 +275,9 @@ namespace KDL { // variables for RotAxis joint Vector axis, origin; - mutable Frame joint_pose; - mutable double q_previous; - + mutable Frame joint_pose; // Deprecated, but keeping for ABI compatibility + mutable double q_previous; // Deprecated, but keeping for ABI compatibility - class joint_type_exception: public std::exception{ virtual const char* what() const throw(){ return "Joint Type excption";} diff --git a/orocos_kdl/src/kinfam.hpp b/orocos_kdl/src/kinfam.hpp index e3eb12f9a..1800c21d8 100644 --- a/orocos_kdl/src/kinfam.hpp +++ b/orocos_kdl/src/kinfam.hpp @@ -25,7 +25,7 @@ * * The Kinematic Families classes range from the basic building blocks * (KDL::Joint and KDL::Segment) and their interconnected kinematic - * structures (KDL::Chain, KDL::Tree and KDL::Graph), to the solver + * structures (KDL::Chain and KDL::Tree), to the solver * algorithms for the kinematics and dynamics of particular kinematic * families. * @@ -46,7 +46,7 @@ * defined by a KDL::Frame), that represents the geometric pose * between the KDL::Joint on the previous segment and its own KDL::Joint. * - * A list of all the classes is available on the modules page: \ref KinFam + * A list of all the classes is available on the modules page: \ref KinematicFamily * * */ diff --git a/orocos_kdl/src/kinfam_io.cpp b/orocos_kdl/src/kinfam_io.cpp index 590484572..d8eb4d31b 100644 --- a/orocos_kdl/src/kinfam_io.cpp +++ b/orocos_kdl/src/kinfam_io.cpp @@ -21,6 +21,7 @@ #include "kinfam_io.hpp" #include "frames_io.hpp" +#include namespace KDL { std::ostream& operator <<(std::ostream& os, const Joint& joint) { @@ -111,5 +112,21 @@ std::ostream& operator <<(std::ostream& os, const JntSpaceInertiaMatrix& jntspac std::istream& operator >>(std::istream& is, JntSpaceInertiaMatrix& jntspaceinertiamatrix) { return is; } + + +std::string tree2str(const SegmentMap::const_iterator it, const std::string& separator, const std::string& preamble, unsigned int level) { + std::stringstream out; + out << preamble; + for(unsigned int i=0; ifirst << "(q_nr: " << GetTreeElementQNr(it->second) << ")\n"; + for (unsigned int i = 0; i < GetTreeElementChildren(it->second).size(); i++) + out << tree2str(GetTreeElementChildren(it->second)[i], separator, preamble, level+1); + return out.str(); } +std::string tree2str(const Tree& tree, const std::string& separator, const std::string& preamble) { + return tree2str(tree.getRootSegment(), separator, preamble, 0); +} + +} diff --git a/orocos_kdl/src/kinfam_io.hpp b/orocos_kdl/src/kinfam_io.hpp index c677c20af..da0ec1e26 100644 --- a/orocos_kdl/src/kinfam_io.hpp +++ b/orocos_kdl/src/kinfam_io.hpp @@ -53,6 +53,13 @@ std::istream& operator >>(std::istream& is, Jacobian& jac); std::ostream& operator <<(std::ostream& os, const JntSpaceInertiaMatrix& jntspaceinertiamatrix); std::istream& operator >>(std::istream& is, JntSpaceInertiaMatrix& jntspaceinertiamatrix); +//Builds a string containing the "branches" of a Tree using indentation or another +//user-supplied pattern, so that it is easier to visualize its structure. It is +//also possible to specify a "preamble", ie, a string to be included at the +//beginning of each new line. +std::string tree2str(const Tree& tree, const std::string& separator=" ", const std::string& preamble=""); +std::string tree2str(const SegmentMap::const_iterator it, const std::string& separator=" ", const std::string& preamble="", unsigned int level=0); + /* template std::ostream& operator<<(std::ostream& os, const std::vector& vec) { @@ -72,4 +79,3 @@ std::istream& operator >>(std::istream& is, std::vector& vec) { */ } #endif - diff --git a/orocos_kdl/src/path.cpp b/orocos_kdl/src/path.cpp index 9b14b0813..b51a49080 100644 --- a/orocos_kdl/src/path.cpp +++ b/orocos_kdl/src/path.cpp @@ -43,6 +43,7 @@ #include "utilities/error.h" #include "utilities/error_stack.h" +#include "utilities/scoped_ptr.hpp" #include "path.hpp" #include "path_line.hpp" #include "path_point.hpp" @@ -55,10 +56,8 @@ namespace KDL { -using namespace std; - -Path* Path::Read(istream& is) { +Path* Path::Read(std::istream& is) { // auto_ptr because exception can be thrown ! IOTrace("Path::Read"); char storage[64]; @@ -78,7 +77,7 @@ Path* Path::Read(istream& is) { Frame endpos; is >> startpos; is >> endpos; - auto_ptr orient( RotationalInterpolation::Read(is) ); + scoped_ptr orient( RotationalInterpolation::Read(is) ); double eqradius; is >> eqradius; EatEnd(is,']'); @@ -99,7 +98,7 @@ Path* Path::Read(istream& is) { is >> R_base_end; is >> alpha; alpha *= deg2rad; - auto_ptr orient( RotationalInterpolation::Read(is) ); + scoped_ptr orient( RotationalInterpolation::Read(is) ); is >> eqradius; EatEnd(is,']'); IOTracePop(); @@ -119,8 +118,8 @@ Path* Path::Read(istream& is) { is >> radius; double eqradius; is >> eqradius; - auto_ptr orient( RotationalInterpolation::Read(is) ); - auto_ptr tr( + scoped_ptr orient( RotationalInterpolation::Read(is) ); + scoped_ptr tr( new Path_RoundedComposite(radius,eqradius,orient.release()) ); int size; @@ -139,7 +138,7 @@ Path* Path::Read(istream& is) { } else if (strcmp(storage,"COMPOSITE")==0) { IOTrace("COMPOSITE"); int size; - auto_ptr tr( new Path_Composite() ); + scoped_ptr tr( new Path_Composite() ); is >> size; int i; for (i=0;i tr( Path::Read(is) ); + scoped_ptr tr( Path::Read(is) ); is >> times; EatEnd(is,']'); IOTracePop(); @@ -164,7 +163,5 @@ Path* Path::Read(istream& is) { return NULL; // just to avoid the warning; } - - } diff --git a/orocos_kdl/src/path_circle.cpp b/orocos_kdl/src/path_circle.cpp index 5efd22bc8..53261e832 100644 --- a/orocos_kdl/src/path_circle.cpp +++ b/orocos_kdl/src/path_circle.cpp @@ -45,51 +45,56 @@ namespace KDL { - - Path_Circle::Path_Circle(const Frame& F_base_start, - const Vector& _V_base_center, - const Vector& V_base_p, - const Rotation& R_base_end, - double alpha, - RotationalInterpolation* _orient, - double _eqradius, - bool _aggregate) : - orient(_orient) , - eqradius(_eqradius), - aggregate(_aggregate) - { - F_base_center.p = _V_base_center; - orient->SetStartEnd(F_base_start.M,R_base_end); - double oalpha = orient->Angle(); - - Vector x(F_base_start.p - F_base_center.p); - radius = x.Normalize(); - if (radius < epsilon) throw Error_MotionPlanning_Circle_ToSmall(); - Vector tmpv(V_base_p-F_base_center.p); - tmpv.Normalize(); - Vector z( x * tmpv); - double n = z.Normalize(); - if (n < epsilon) throw Error_MotionPlanning_Circle_No_Plane(); - F_base_center.M = Rotation(x,z*x,z); - double dist = alpha*radius; - // See what has the slowest eq. motion, and adapt - // the other to this slower motion - // use eqradius to transform between rot and transl. - // the same as for lineair motion - if (oalpha*eqradius > dist) { - // rotational_interpolation is the limitation - pathlength = oalpha*eqradius; - scalerot = 1/eqradius; - scalelin = dist/pathlength; - } else { - // translation is the limitation - pathlength = dist; - scalerot = oalpha/pathlength; - scalelin = 1; - } - } - + const Vector& _V_base_center, + const Vector& V_base_p, + const Rotation& R_base_end, + double alpha, + RotationalInterpolation* _orient, + double _eqradius, + bool _aggregate) : + orient(_orient) , + eqradius(_eqradius), + aggregate(_aggregate) +{ + F_base_center.p = _V_base_center; + orient->SetStartEnd(F_base_start.M,R_base_end); + double oalpha = orient->Angle(); + + Vector x(F_base_start.p - F_base_center.p); + radius = x.Normalize(); + if (radius < epsilon) { + if (aggregate) + delete orient; + throw Error_MotionPlanning_Circle_ToSmall(); + } + Vector tmpv(V_base_p-F_base_center.p); + tmpv.Normalize(); + Vector z( x * tmpv); + double n = z.Normalize(); + if (n < epsilon) { + if (aggregate) + delete orient; + throw Error_MotionPlanning_Circle_No_Plane(); + } + F_base_center.M = Rotation(x,z*x,z); + double dist = alpha*radius; + // See what has the slowest eq. motion, and adapt + // the other to this slower motion + // use eqradius to transform between rot and transl. + // the same as for lineair motion + if (oalpha*eqradius > dist) { + // rotational_interpolation is the limitation + pathlength = oalpha*eqradius; + scalerot = 1/eqradius; + scalelin = dist/pathlength; + } else { + // translation is the limitation + pathlength = dist; + scalerot = oalpha/pathlength; + scalelin = 1; + } +} double Path_Circle::LengthToS(double length) { @@ -150,8 +155,6 @@ Path_Circle::~Path_Circle() { delete orient; } - - void Path_Circle::Write(std::ostream& os) { os << "CIRCLE[ "; os << " " << Pos(0) << std::endl; @@ -164,6 +167,4 @@ void Path_Circle::Write(std::ostream& os) { os << "]"<< std::endl; } - } - diff --git a/orocos_kdl/src/path_composite.cpp b/orocos_kdl/src/path_composite.cpp index 4df5f3929..c93f5a293 100644 --- a/orocos_kdl/src/path_composite.cpp +++ b/orocos_kdl/src/path_composite.cpp @@ -42,6 +42,7 @@ #include "path_composite.hpp" #include "utilities/error.h" +#include "utilities/scoped_ptr.hpp" #include namespace KDL { @@ -110,7 +111,7 @@ Twist Path_Composite::Acc(double s,double sd,double sdd) const { } Path* Path_Composite::Clone() { - std::auto_ptr comp( new Path_Composite() ); + scoped_ptr comp( new Path_Composite() ); for (unsigned int i = 0; i < dv.size(); ++i) { comp->Add(gv[i].first->Clone(), gv[i].second); } @@ -127,18 +128,18 @@ void Path_Composite::Write(std::ostream& os) { } int Path_Composite::GetNrOfSegments() { - return dv.size(); + return static_cast(dv.size()); } Path* Path_Composite::GetSegment(int i) { assert(i>=0); - assert(i(dv.size())); return gv[i].first; } double Path_Composite::GetLengthToEndOfSegment(int i) { assert(i>=0); - assert(i(dv.size())); return dv[i]; } diff --git a/orocos_kdl/src/path_roundedcomposite.cpp b/orocos_kdl/src/path_roundedcomposite.cpp index dbc67b05f..2916505c0 100644 --- a/orocos_kdl/src/path_roundedcomposite.cpp +++ b/orocos_kdl/src/path_roundedcomposite.cpp @@ -44,6 +44,7 @@ #include "path_line.hpp" #include "path_circle.hpp" #include "utilities/error.h" +#include "utilities/scoped_ptr.hpp" #include @@ -85,7 +86,7 @@ void Path_RoundedComposite::Add(const Frame& F_base_point) { if (bcdist < eps) { throw Error_MotionPlanning_Not_Feasible(3); } - // Clamp to avoid rounding errors (acos is defineed between [-1 ; 1]) + // Clamp to avoid rounding errors (acos is defined between [-1 ; 1]) double alpha = acos(std::max(-1., std::min(dot(ab, bc) / abdist / bcdist, 1.))); if ((PI - alpha) < eps) { throw Error_MotionPlanning_Not_Feasible(4); @@ -105,11 +106,11 @@ void Path_RoundedComposite::Add(const Frame& F_base_point) { if (d >= bcdist) throw Error_MotionPlanning_Not_Feasible(6); - std::auto_ptr < Path + scoped_ptr < Path > line1( new Path_Line(F_base_start, F_base_via, orient->Clone(), eqradius)); - std::auto_ptr < Path + scoped_ptr < Path > line2( new Path_Line(F_base_via, F_base_point, orient->Clone(), eqradius)); @@ -188,8 +189,8 @@ void Path_RoundedComposite::GetCurrentSegmentLocation(double s, Path_RoundedComposite::~Path_RoundedComposite() { - if (aggregate) - delete orient; + if (aggregate) + delete orient; delete comp; } diff --git a/orocos_kdl/src/path_roundedcomposite.hpp b/orocos_kdl/src/path_roundedcomposite.hpp index 79c2657f5..a7451359d 100644 --- a/orocos_kdl/src/path_roundedcomposite.hpp +++ b/orocos_kdl/src/path_roundedcomposite.hpp @@ -88,7 +88,7 @@ class Path_RoundedComposite : public Path Path_RoundedComposite(double radius,double eqradius,RotationalInterpolation* orient, bool aggregate=true); /** - * Adds a point to this rounded composite, between to adjecent points + * Adds a point to this rounded composite, between two adjacent points * a Path_Line will be created, between two lines there will be * rounding with the given radius with a Path_Circle * @@ -96,7 +96,7 @@ class Path_RoundedComposite : public Path * - 3101 if the eq. radius <= 0 * - 3102 if the first segment in a rounding has zero length. * - 3103 if the second segment in a rounding has zero length. - * - 3104 if the angle between the first and the second segment is close to M_PI. + * - 3104 if the angle between the first and the second segment is close to PI. * (meaning that the segments are on top of each other) * - 3105 if the distance needed for the rounding is larger then the first segment. * - 3106 if the distance needed for the rounding is larger then the second segment. diff --git a/orocos_kdl/src/rigidbodyinertia.cpp b/orocos_kdl/src/rigidbodyinertia.cpp index 2313fce56..3bfd938d0 100644 --- a/orocos_kdl/src/rigidbodyinertia.cpp +++ b/orocos_kdl/src/rigidbodyinertia.cpp @@ -23,8 +23,6 @@ #include -using namespace Eigen; - namespace KDL{ const static bool mhi=true; @@ -37,8 +35,8 @@ namespace KDL{ RigidBodyInertia::RigidBodyInertia(double m_, const Vector& c_, const RotationalInertia& Ic): m(m_),h(m*c_){ //I=Ic-c x c x - Vector3d c_eig=Map(c_.data); - Map(I.data)=Map(Ic.data)-m_*(c_eig*c_eig.transpose()-c_eig.dot(c_eig)*Matrix3d::Identity()); + Eigen::Vector3d c_eig=Eigen::Map(c_.data); + Eigen::Map(I.data)=Eigen::Map(Ic.data)-m_*(c_eig*c_eig.transpose()-c_eig.dot(c_eig)*Eigen::Matrix3d::Identity()); } RigidBodyInertia operator*(double a,const RigidBodyInertia& I){ @@ -59,14 +57,14 @@ namespace KDL{ //hb=R*(h-m*r) //Ib = R(Ia+r x h x + (h-m*r) x r x)R' Vector hmr = (I.h-I.m*X.p); - Vector3d r_eig = Map(X.p.data); - Vector3d h_eig = Map(I.h.data); - Vector3d hmr_eig = Map(hmr.data); - Matrix3d rcrosshcross = h_eig *r_eig.transpose()-r_eig.dot(h_eig)*Matrix3d::Identity(); - Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Matrix3d::Identity(); - Matrix3d R = Map(X.M.data); + Eigen::Vector3d r_eig = Eigen::Map(X.p.data); + Eigen::Vector3d h_eig = Eigen::Map(I.h.data); + Eigen::Vector3d hmr_eig = Eigen::Map(hmr.data); + Eigen::Matrix3d rcrosshcross = h_eig *r_eig.transpose()-r_eig.dot(h_eig)*Eigen::Matrix3d::Identity(); + Eigen::Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Eigen::Matrix3d::Identity(); + Eigen::Matrix3d R = Eigen::Map(X.M.data); RotationalInertia Ib; - Map(Ib.data) = R*((Map(I.I.data)+rcrosshcross+hmrcrossrcross)*R.transpose()); + Eigen::Map(Ib.data) = R*((Eigen::Map(I.I.data)+rcrosshcross+hmrcrossrcross)*R.transpose()); return RigidBodyInertia(I.m,T.M*hmr,Ib,mhi); } @@ -75,9 +73,9 @@ namespace KDL{ //mb=ma //hb=R*h //Ib = R(Ia)R' with r=0 - Matrix3d R = Map(M.data); + Eigen::Matrix3d R = Eigen::Map(M.data); RotationalInertia Ib; - Map(Ib.data) = R.transpose()*(Map(I.I.data)*R); + Eigen::Map(Ib.data) = R.transpose()*(Eigen::Map(I.I.data)*R); return RigidBodyInertia(I.m,M*I.h,Ib,mhi); } @@ -87,13 +85,13 @@ namespace KDL{ //hb=(h-m*r) //Ib = (Ia+r x h x + (h-m*r) x r x) Vector hmr = (this->h-this->m*p); - Vector3d r_eig = Map(p.data); - Vector3d h_eig = Map(this->h.data); - Vector3d hmr_eig = Map(hmr.data); - Matrix3d rcrosshcross = h_eig * r_eig.transpose()-r_eig.dot(h_eig)*Matrix3d::Identity(); - Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Matrix3d::Identity(); + Eigen::Vector3d r_eig = Eigen::Map(p.data); + Eigen::Vector3d h_eig = Eigen::Map(this->h.data); + Eigen::Vector3d hmr_eig = Eigen::Map(hmr.data); + Eigen::Matrix3d rcrosshcross = h_eig * r_eig.transpose()-r_eig.dot(h_eig)*Eigen::Matrix3d::Identity(); + Eigen::Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Eigen::Matrix3d::Identity(); RotationalInertia Ib; - Map(Ib.data) = Map(this->I.data)+rcrosshcross+hmrcrossrcross; + Eigen::Map(Ib.data) = Eigen::Map(this->I.data)+rcrosshcross+hmrcrossrcross; return RigidBodyInertia(this->m,hmr,Ib,mhi); } diff --git a/orocos_kdl/src/rigidbodyinertia.hpp b/orocos_kdl/src/rigidbodyinertia.hpp index 8992c4f9a..42fc5d0fd 100644 --- a/orocos_kdl/src/rigidbodyinertia.hpp +++ b/orocos_kdl/src/rigidbodyinertia.hpp @@ -71,7 +71,15 @@ namespace KDL { double getMass() const{ return m; }; - + + /** + * Get the spatial momentum of the rigid body + */ + const Vector& getSpatialMomentum() const + { + return h; + } + /** * Get the center of gravity of the rigid body */ diff --git a/orocos_kdl/src/rotational_interpolation.cpp b/orocos_kdl/src/rotational_interpolation.cpp index f8186e67a..74999ae92 100644 --- a/orocos_kdl/src/rotational_interpolation.cpp +++ b/orocos_kdl/src/rotational_interpolation.cpp @@ -48,10 +48,7 @@ namespace KDL { -using namespace std; - -RotationalInterpolation* RotationalInterpolation::Read(istream& is) { - // auto_ptr because exception can be thrown ! +RotationalInterpolation* RotationalInterpolation::Read(std::istream& is) { IOTrace("RotationalInterpolation::Read"); char storage[64]; EatWord(is,"[",storage,sizeof(storage)); @@ -83,4 +80,3 @@ RotationalInterpolation* RotationalInterpolation::Read(istream& is) { } } - diff --git a/orocos_kdl/src/rotational_interpolation_sa.cpp b/orocos_kdl/src/rotational_interpolation_sa.cpp index 6a0accd14..6b5a56b5c 100644 --- a/orocos_kdl/src/rotational_interpolation_sa.cpp +++ b/orocos_kdl/src/rotational_interpolation_sa.cpp @@ -47,7 +47,7 @@ namespace KDL { RotationalInterpolation_SingleAxis::RotationalInterpolation_SingleAxis() - {}; + {} void RotationalInterpolation_SingleAxis::SetStartEnd(Rotation start,Rotation end) { R_base_start = start; diff --git a/orocos_kdl/src/rotationalinertia.cpp b/orocos_kdl/src/rotationalinertia.cpp index 5210d6c72..f52c0ecc6 100644 --- a/orocos_kdl/src/rotationalinertia.cpp +++ b/orocos_kdl/src/rotationalinertia.cpp @@ -21,7 +21,7 @@ #include "rotationalinertia.hpp" #include -using namespace Eigen; + namespace KDL { @@ -43,19 +43,19 @@ namespace KDL Vector RotationalInertia::operator*(const Vector& omega) const { // Complexity : 9M+6A Vector result; - Map(result.data)=Map(this->data)*Map(omega.data); + Eigen::Map(result.data) = Eigen::Map(this->data) * Eigen::Map(omega.data); return result; } RotationalInertia operator*(double a, const RotationalInertia& I){ RotationalInertia result; - Map(result.data)=a*Map(I.data); + Eigen::Map(result.data) = a * Eigen::Map(I.data); return result; } RotationalInertia operator+(const RotationalInertia& Ia, const RotationalInertia& Ib){ RotationalInertia result; - Map(result.data)=Map(Ia.data)+Map(Ib.data); + Eigen::Map(result.data) = Eigen::Map(Ia.data) + Eigen::Map(Ib.data); return result; } } diff --git a/orocos_kdl/src/segment.hpp b/orocos_kdl/src/segment.hpp index 934b17a07..800076361 100644 --- a/orocos_kdl/src/segment.hpp +++ b/orocos_kdl/src/segment.hpp @@ -1,8 +1,8 @@ -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2007 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -57,22 +57,22 @@ namespace KDL { * * @param name name of the segment * @param joint joint of the segment, default: - * Joint(Joint::None) + * Joint(Joint::Fixed) * @param f_tip frame from the end of the joint to the tip of * the segment, default: Frame::Identity() * @param M rigid body inertia of the segment, default: Inertia::Zero() */ - explicit Segment(const std::string& name, const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); + explicit Segment(const std::string& name, const Joint& joint=Joint(Joint::Fixed), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); /** * Constructor of the segment * * @param joint joint of the segment, default: - * Joint(Joint::None) + * Joint(Joint::Fixed) * @param f_tip frame from the end of the joint to the tip of * the segment, default: Frame::Identity() * @param M rigid body inertia of the segment, default: Inertia::Zero() */ - explicit Segment(const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); + explicit Segment(const Joint& joint=Joint(Joint::Fixed), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); Segment(const Segment& in); Segment& operator=(const Segment& arg); @@ -148,7 +148,7 @@ namespace KDL { */ Frame getFrameToTip()const { - + return joint.pose(0)*f_tip; } @@ -159,6 +159,18 @@ namespace KDL { * @param f_tip_new pose from the joint end to the tip of the segment */ void setFrameToTip(const Frame& f_tip_new); + + /** + * Request the pose from the end of the joint to the tip of the segment + * at joint position 0. + * + * @return const reference to the pose from the end of the joint to the tip of the segment + * at joint position 0 + */ + const Frame& getFrameToTipZero() const + { + return f_tip; + } }; }//end of namespace KDL diff --git a/orocos_kdl/src/trajectory.cpp b/orocos_kdl/src/trajectory.cpp index 6a3e78639..914af5f63 100644 --- a/orocos_kdl/src/trajectory.cpp +++ b/orocos_kdl/src/trajectory.cpp @@ -42,6 +42,7 @@ #include "utilities/error.h" #include "utilities/error_stack.h" +#include "utilities/scoped_ptr.hpp" #include "trajectory.hpp" #include "path.hpp" #include "trajectory_segment.hpp" @@ -52,18 +53,15 @@ namespace KDL { -using namespace std; - Trajectory* Trajectory::Read(std::istream& is) { - // auto_ptr because exception can be thrown ! IOTrace("Trajectory::Read"); char storage[64]; EatWord(is,"[",storage,sizeof(storage)); Eat(is,'['); if (strcmp(storage,"SEGMENT")==0) { IOTrace("SEGMENT"); - auto_ptr geom( Path::Read(is) ); - auto_ptr motprof( VelocityProfile::Read(is) ); + scoped_ptr geom( Path::Read(is) ); + scoped_ptr motprof( VelocityProfile::Read(is) ); EatEnd(is,']'); IOTracePop(); IOTracePop(); @@ -74,7 +72,4 @@ Trajectory* Trajectory::Read(std::istream& is) { return NULL; // just to avoid the warning; } - - } - diff --git a/orocos_kdl/src/trajectory_composite.cpp b/orocos_kdl/src/trajectory_composite.cpp index 82f4851b6..158360899 100644 --- a/orocos_kdl/src/trajectory_composite.cpp +++ b/orocos_kdl/src/trajectory_composite.cpp @@ -18,9 +18,6 @@ namespace KDL { - using namespace std; - - Trajectory_Composite::Trajectory_Composite():duration(0.0) { } @@ -107,14 +104,13 @@ namespace KDL { Destroy(); } - - void Trajectory_Composite::Write(ostream& os) const { - os << "COMPOSITE[ " << vt.size() << endl; + void Trajectory_Composite::Write(std::ostream& os) const { + os << "COMPOSITE[ " << vt.size() << std::endl; unsigned int i; for (i=0;iWrite(os); } - os << "]" << endl; + os << "]" << std::endl; } Trajectory* Trajectory_Composite::Clone() const{ @@ -126,5 +122,3 @@ namespace KDL { } } - - diff --git a/orocos_kdl/src/trajectory_stationary.cpp b/orocos_kdl/src/trajectory_stationary.cpp index a2d491bc7..3b712a6fe 100644 --- a/orocos_kdl/src/trajectory_stationary.cpp +++ b/orocos_kdl/src/trajectory_stationary.cpp @@ -18,15 +18,10 @@ namespace KDL { - using namespace std; - - -void Trajectory_Stationary::Write(ostream& os) const { - os << "STATIONARY[ " << duration << endl; - os << pos << endl; +void Trajectory_Stationary::Write(std::ostream& os) const { + os << "STATIONARY[ " << duration << std::endl; + os << pos << std::endl; os << "]"; } - } - diff --git a/orocos_kdl/src/tree.cpp b/orocos_kdl/src/tree.cpp index 1ca751489..70151e663 100644 --- a/orocos_kdl/src/tree.cpp +++ b/orocos_kdl/src/tree.cpp @@ -1,8 +1,8 @@ -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2007 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -23,7 +23,6 @@ #include namespace KDL { -using namespace std; Tree::Tree(const std::string& _root_name) : nrOfJoints(0), nrOfSegments(0), root_name(_root_name) @@ -57,9 +56,9 @@ bool Tree::addSegment(const Segment& segment, const std::string& hook_name) { //check if parent exists if (parent == segments.end()) return false; - pair retval; + std::pair retval; //insert new element - unsigned int q_nr = segment.getJoint().getType() != Joint::None ? nrOfJoints : 0; + unsigned int q_nr = segment.getJoint().getType() != Joint::Fixed ? nrOfJoints : 0; #ifdef KDL_USE_NEW_TREE_INTERFACE retval = segments.insert(make_pair(segment.getName(), TreeElementType( new TreeElement(segment, parent, q_nr)))); @@ -75,13 +74,13 @@ bool Tree::addSegment(const Segment& segment, const std::string& hook_name) { //increase number of segments nrOfSegments++; //increase number of joints - if (segment.getJoint().getType() != Joint::None) + if (segment.getJoint().getType() != Joint::Fixed) nrOfJoints++; return true; } - + bool Tree::addChain(const Chain& chain, const std::string& hook_name) { - string parent_name = hook_name; + std::string parent_name = hook_name; for (unsigned int i = 0; i < chain.getNrOfSegments(); i++) { if (this->addSegment(chain.getSegment(i), parent_name)) parent_name = chain.getSegment(i).getName(); @@ -113,56 +112,65 @@ bool Tree::addTreeRecursive(SegmentMap::const_iterator root, const std::string& } return true; } - - bool Tree::getChain(const std::string& chain_root, const std::string& chain_tip, Chain& chain)const - { - // clear chain - chain = Chain(); - - // walk down from chain_root and chain_tip to the root of the tree - vector parents_chain_root, parents_chain_tip; - for (SegmentMap::const_iterator s=getSegment(chain_root); s!=segments.end(); s = GetTreeElementParent(s->second)){ - parents_chain_root.push_back(s->first); - if (s->first == root_name) break; - } - if (parents_chain_root.empty() || parents_chain_root.back() != root_name) return false; - for (SegmentMap::const_iterator s=getSegment(chain_tip); s!=segments.end(); s = GetTreeElementParent(s->second)){ - parents_chain_tip.push_back(s->first); - if (s->first == root_name) break; - } - if (parents_chain_tip.empty() || parents_chain_tip.back() != root_name) return false; - - // remove common part of segment lists - SegmentMap::key_type last_segment = root_name; - while (!parents_chain_root.empty() && !parents_chain_tip.empty() && - parents_chain_root.back() == parents_chain_tip.back()){ - last_segment = parents_chain_root.back(); - parents_chain_root.pop_back(); - parents_chain_tip.pop_back(); - } - parents_chain_root.push_back(last_segment); - - - // add the segments from the root to the common frame - for (unsigned int s=0; ssecond); - Frame f_tip = seg.pose(0.0).Inverse(); - Joint jnt = seg.getJoint(); - if (jnt.getType() == Joint::RotX || jnt.getType() == Joint::RotY || jnt.getType() == Joint::RotZ || jnt.getType() == Joint::RotAxis) - jnt = Joint(jnt.getName(), f_tip*jnt.JointOrigin(), f_tip.M*(-jnt.JointAxis()), Joint::RotAxis); - else if (jnt.getType() == Joint::TransX || jnt.getType() == Joint::TransY || jnt.getType() == Joint::TransZ || jnt.getType() == Joint::TransAxis) - jnt = Joint(jnt.getName(),f_tip*jnt.JointOrigin(), f_tip.M*(-jnt.JointAxis()), Joint::TransAxis); + +bool Tree::getChain(const std::string& chain_root, const std::string& chain_tip, Chain& chain)const +{ + // clear chain + chain = Chain(); + + // walk down from chain_root and chain_tip to the root of the tree + std::vector parents_chain_root, parents_chain_tip; + for (SegmentMap::const_iterator s=getSegment(chain_root); s!=segments.end(); s = GetTreeElementParent(s->second)){ + parents_chain_root.push_back(s->first); + if (s->first == root_name) break; + } + if (parents_chain_root.empty() || parents_chain_root.back() != root_name) return false; + for (SegmentMap::const_iterator s=getSegment(chain_tip); s!=segments.end(); s = GetTreeElementParent(s->second)){ + parents_chain_tip.push_back(s->first); + if (s->first == root_name) break; + } + if (parents_chain_tip.empty() || parents_chain_tip.back() != root_name) return false; + + // remove common part of segment lists + SegmentMap::key_type last_segment = root_name; + while (!parents_chain_root.empty() && !parents_chain_tip.empty() && + parents_chain_root.back() == parents_chain_tip.back()){ + last_segment = parents_chain_root.back(); + parents_chain_root.pop_back(); + parents_chain_tip.pop_back(); + } + parents_chain_root.push_back(last_segment); + + + // add the segments from the root to the common frame + for (unsigned int s=0; ssecond); + Frame f_tip = seg.pose(0.0).Inverse(); + Joint jnt = seg.getJoint(); + if (jnt.getType() == Joint::RotX || jnt.getType() == Joint::RotY || jnt.getType() == Joint::RotZ || jnt.getType() == Joint::RotAxis) + jnt = Joint(jnt.getName(), f_tip*jnt.JointOrigin(), f_tip.M*(-jnt.JointAxis()), Joint::RotAxis); + else if (jnt.getType() == Joint::TransX || jnt.getType() == Joint::TransY || jnt.getType() == Joint::TransZ || jnt.getType() == Joint::TransAxis) + jnt = Joint(jnt.getName(),f_tip*jnt.JointOrigin(), f_tip.M*(-jnt.JointAxis()), Joint::TransAxis); chain.addSegment(Segment(GetTreeElementSegment(getSegment(parents_chain_root[s+1])->second).getName(), - jnt, f_tip, GetTreeElementSegment(getSegment(parents_chain_root[s+1])->second).getInertia())); - } - - // add the segments from the common frame to the tip frame - for (int s=parents_chain_tip.size()-1; s>-1; s--){ - chain.addSegment(GetTreeElementSegment(getSegment(parents_chain_tip[s])->second)); - } - return true; + jnt, f_tip, GetTreeElementSegment(getSegment(parents_chain_root[s+1])->second).getInertia())); } - -}//end of namespace + // add the segments from the common frame to the tip frame + for (auto rit=parents_chain_tip.rbegin(); rit != parents_chain_tip.rend(); ++rit){ + chain.addSegment(GetTreeElementSegment(getSegment(*rit)->second)); + } + return true; +} +bool Tree::getSubTree(const std::string& segment_name, Tree& tree) const +{ + //check if segment_name exists + SegmentMap::const_iterator root = segments.find(segment_name); + if (root == segments.end()) + return false; + //init the tree, segment_name is the new root. + tree = Tree(root->first); + return tree.addTreeRecursive(root, segment_name); +} + +}//end of namespace diff --git a/orocos_kdl/src/tree.hpp b/orocos_kdl/src/tree.hpp index 3e33cc7f2..fd664e879 100644 --- a/orocos_kdl/src/tree.hpp +++ b/orocos_kdl/src/tree.hpp @@ -129,7 +129,7 @@ namespace KDL /** * Adds a complete chain to the end of the segment with - * hook_name as segment_name. + * hook_name as segment_name. * * @param hook_name name of the segment to connect the chain with. * @@ -139,7 +139,7 @@ namespace KDL /** * Adds a complete tree to the end of the segment with - * hookname as segment_name. + * hookname as segment_name. * * @param tree Tree to add * @param hook_name name of the segment to connect the tree with @@ -188,19 +188,32 @@ namespace KDL return segments.find(root_name); }; - /** - * Request the chain of the tree between chain_root and chain_tip. The chain_root - * and chain_tip can be in different branches of the tree, the chain_root can be - * an ancestor of chain_tip, and chain_tip can be an ancestor of chain_root. - * - * @param chain_root the name of the root segment of the chain - * @param chain_tip the name of the tip segment of the chain - * @param chain the resulting chain - * - * @return success or failure - */ - bool getChain(const std::string& chain_root, const std::string& chain_tip, Chain& chain)const; - + /** + * Request the chain of the tree between chain_root and chain_tip. The chain_root + * and chain_tip can be in different branches of the tree, the chain_root can be + * an ancestor of chain_tip, and chain_tip can be an ancestor of chain_root. + * + * @param chain_root the name of the root segment of the chain + * @param chain_tip the name of the tip segment of the chain + * @param chain the resulting chain + * + * @return success or failure + */ + bool getChain(const std::string& chain_root, const std::string& chain_tip, Chain& chain)const; + + + + /** + * Extract a tree having segment_name as root. Only child segments of + * segment_name are added to the new tree. + * + * @param segment_name the name of the segment to be used as root + * of the new tree + * @param tree the resulting sub-tree + * + * @return success or failure + */ + bool getSubTree(const std::string& segment_name, Tree& tree)const; const SegmentMap& getSegments()const { @@ -212,8 +225,3 @@ namespace KDL }; } #endif - - - - - diff --git a/orocos_kdl/src/treefksolver.hpp b/orocos_kdl/src/treefksolver.hpp index 3d521d340..80ca1fc70 100644 --- a/orocos_kdl/src/treefksolver.hpp +++ b/orocos_kdl/src/treefksolver.hpp @@ -89,7 +89,7 @@ namespace KDL { // class TreeFkSolverAcc { // public: /** - * Calculate forward position, velocity and accelaration + * Calculate forward position, velocity and acceleration * kinematics, from joint coordinates to cartesian coordinates * * @param q_in input joint coordinates (position, velocity and diff --git a/orocos_kdl/src/treeidsolver.hpp b/orocos_kdl/src/treeidsolver.hpp new file mode 100644 index 000000000..c2751e2fc --- /dev/null +++ b/orocos_kdl/src/treeidsolver.hpp @@ -0,0 +1,62 @@ +// Copyright (C) 2009 Ruben Smits + +// Version: 1.0 +// Author: Franco Fusco +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_TREE_IDSOLVER_HPP +#define KDL_TREE_IDSOLVER_HPP + +#include "tree.hpp" +#include "frames.hpp" +#include "jntarray.hpp" +#include "solveri.hpp" + +namespace KDL +{ + + typedef std::map WrenchMap; + + /** + * \brief This abstract class encapsulates the inverse + * dynamics solver for a KDL::Tree. + * + */ + class TreeIdSolver : public KDL::SolverI + { + public: + /** + * Calculate inverse dynamics, from joint positions, velocity, acceleration, external forces + * to joint torques/forces. + * + * @param q input joint positions + * @param q_dot input joint velocities + * @param q_dotdot input joint accelerations + * @param f_ext the external forces (no gravity) on the segments + * @param torque output joint torques + * + * @return if < 0 something went wrong + */ + virtual int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext,JntArray &torques)=0; + + // Need functions to return the manipulator mass, coriolis and gravity matrices - Lagrangian Formulation. + }; + +} + +#endif diff --git a/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp b/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp new file mode 100644 index 000000000..b26fb3213 --- /dev/null +++ b/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp @@ -0,0 +1,139 @@ +// Copyright (C) 2009 Ruben Smits + +// Version: 1.0 +// Author: Franco Fusco +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "treeidsolver_recursive_newton_euler.hpp" +#include "frames_io.hpp" +#include + +namespace KDL{ + + TreeIdSolver_RNE::TreeIdSolver_RNE(const Tree& tree_, Vector grav): + tree(tree_), nj(tree.getNrOfJoints()), ns(tree.getNrOfSegments()) + { + ag=-Twist(grav,Vector::Zero()); + initAuxVariables(); + } + + void TreeIdSolver_RNE::updateInternalDataStructures() { + nj = tree.getNrOfJoints(); + ns = tree.getNrOfSegments(); + initAuxVariables(); + } + + void TreeIdSolver_RNE::initAuxVariables() { + const SegmentMap& segments = tree.getSegments(); + for(SegmentMap::const_iterator seg = segments.begin(); seg != segments.end(); seg++) { + X[seg->first] = Frame(); + S[seg->first] = Twist(); + v[seg->first] = Twist(); + a[seg->first] = Twist(); + f[seg->first] = Wrench(); + } + } + + int TreeIdSolver_RNE::CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray &torques) + { + //Check that the tree was not modified externally + if(nj != tree.getNrOfJoints() || ns != tree.getNrOfSegments()) + return (error = E_NOT_UP_TO_DATE); + + //Check sizes of joint vectors + if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj) + return (error = E_SIZE_MISMATCH); + + try { + //Do the recursion here + rne_step(tree.getRootSegment(), q, q_dot, q_dotdot, f_ext, torques); + } + catch(const std::out_of_range&) { + //If in rne_step we get an out_of_range exception it means that some call + //to map::at failed. This can happen only if updateInternalDataStructures + //was not called after changing something in the tree. Note that it + //should be impossible to reach this point as consistency of the tree is + //checked above. + return (error = E_NOT_UP_TO_DATE); + } + return (error = E_NOERROR); + } + + + void TreeIdSolver_RNE::rne_step(SegmentMap::const_iterator segment, const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray& torques) { + const Segment& seg = GetTreeElementSegment(segment->second); + const std::string& segname = segment->first; + const std::string& parname = GetTreeElementParent(segment->second)->first; + + //Do forward calculations involving velocity & acceleration of this segment + double q_, qdot_, qdotdot_; + unsigned int j = GetTreeElementQNr(segment->second); + if(seg.getJoint().getType()!=Joint::Fixed) { + q_ = q(j); + qdot_ = q_dot(j); + qdotdot_ = q_dotdot(j); + } + else + q_ = qdot_ = qdotdot_ = 0.0; + + //Calculate segment properties: X,S,vj,cj + + //Remark this is the inverse of the frame for transformations from the parent to the current coord frame + X.at(segname) = seg.pose(q_); + + //Transform velocity and unit velocity to segment frame + Twist vj = X.at(segname).M.Inverse( seg.twist(q_,qdot_) ); + S.at(segname) = X.at(segname).M.Inverse( seg.twist(q_,1.0) ); + + //calculate velocity and acceleration of the segment (in segment coordinates) + if(segment == tree.getRootSegment()) { + v.at(segname) = vj; + a.at(segname) = X.at(segname).Inverse(ag) + S.at(segname)*qdotdot_+ v.at(segname)*vj; + } + else { + v.at(segname) = X.at(segname).Inverse(v.at(parname)) + vj; + a.at(segname) = X.at(segname).Inverse(a.at(parname)) + S.at(segname)*qdotdot_ + v.at(segname)*vj; + } + + //Calculate the force for the joint + //Collect RigidBodyInertia and external forces + const RigidBodyInertia& I = seg.getInertia(); + f.at(segname) = I*a.at(segname) + v.at(segname)*(I*v.at(segname)); + if(f_ext.find(segname) != f_ext.end()) + f.at(segname) = f.at(segname) - f_ext.at(segname); + + //propagate calculations over each child segment + SegmentMap::const_iterator child; + for (unsigned int i = 0; i < GetTreeElementChildren(segment->second).size(); i++) { + child = GetTreeElementChildren(segment->second)[i]; + rne_step(child, q, q_dot, q_dotdot, f_ext, torques); + } + + //do backward calculations involving wrenches and joint efforts + + //If there is a moving joint, evaluate its effort + if(seg.getJoint().getType()!=Joint::Fixed) { + torques(j) = dot(S.at(segname), f.at(segname)); + torques(j) += seg.getJoint().getInertia()*q_dotdot(j); // add torque from joint inertia + } + + //add reaction forces to parent segment + if(segment != tree.getRootSegment()) + f.at(parname) = f.at(parname) + X.at(segname)*f.at(segname); + } +}//namespace diff --git a/orocos_kdl/src/treeidsolver_recursive_newton_euler.hpp b/orocos_kdl/src/treeidsolver_recursive_newton_euler.hpp new file mode 100644 index 000000000..f2697b19a --- /dev/null +++ b/orocos_kdl/src/treeidsolver_recursive_newton_euler.hpp @@ -0,0 +1,84 @@ +// Copyright (C) 2009 Ruben Smits + +// Version: 1.0 +// Author: Franco Fusco +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_TREE_IDSOLVER_RECURSIVE_NEWTON_EULER_HPP +#define KDL_TREE_IDSOLVER_RECURSIVE_NEWTON_EULER_HPP + +#include "treeidsolver.hpp" + +namespace KDL{ + /** + * \brief Recursive newton euler inverse dynamics solver for kinematic trees. + * + * It calculates the torques for the joints, given the motion of + * the joints (q,qdot,qdotdot), external forces on the segments + * (expressed in the segments reference frame) and the dynamical + * parameters of the segments. + * + * This is an extension of the inverse dynamic solver for kinematic chains, + * \see ChainIdSolver_RNE. The main difference is the use of STL maps + * instead of vectors to represent external wrenches (as well as internal + * variables exploited during the recursion). + */ + class TreeIdSolver_RNE : public TreeIdSolver { + public: + /** + * Constructor for the solver, it will allocate all the necessary memory + * \param tree The kinematic tree to calculate the inverse dynamics for, an internal reference will be stored. + * \param grav The gravity vector to use during the calculation. + */ + TreeIdSolver_RNE(const Tree& tree, Vector grav); + + /** + * Function to calculate from Cartesian forces to joint torques. + * Input parameters; + * \param q The current joint positions + * \param q_dot The current joint velocities + * \param q_dotdot The current joint accelerations + * \param f_ext The external forces (no gravity) on the segments + * Output parameters: + * \param torques the resulting torques for the joints + */ + int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray &torques); + + /// @copydoc KDL::SolverI::updateInternalDataStructures + virtual void updateInternalDataStructures(); + + private: + ///Helper function to initialize private members X, S, v, a, f + void initAuxVariables(); + + ///One recursion step + void rne_step(SegmentMap::const_iterator segment, const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray& torques); + + const Tree& tree; + unsigned int nj; + unsigned int ns; + std::map X; + std::map S; + std::map v; + std::map a; + std::map f; + Twist ag; + }; +} + +#endif diff --git a/orocos_kdl/src/treeiksolverpos_online.cpp b/orocos_kdl/src/treeiksolverpos_online.cpp index 3e3d1318d..594c3a056 100644 --- a/orocos_kdl/src/treeiksolverpos_online.cpp +++ b/orocos_kdl/src/treeiksolverpos_online.cpp @@ -35,12 +35,9 @@ TreeIkSolverPos_Online::TreeIkSolverPos_Online(const double& nr_of_jnts, const double x_dot_rot_max, TreeFkSolverPos& fksolver, TreeIkSolverVel& iksolver) : - q_min_(nr_of_jnts), - q_max_(nr_of_jnts), - q_dot_max_(nr_of_jnts), fksolver_(fksolver), iksolver_(iksolver), - q_dot_(nr_of_jnts) + q_dot_(static_cast(nr_of_jnts)) { q_min_ = q_min; q_max_ = q_max; diff --git a/orocos_kdl/src/treeiksolvervel_wdls.cpp b/orocos_kdl/src/treeiksolvervel_wdls.cpp index 924566ab8..87665dbd3 100644 --- a/orocos_kdl/src/treeiksolvervel_wdls.cpp +++ b/orocos_kdl/src/treeiksolvervel_wdls.cpp @@ -8,21 +8,19 @@ #include "treeiksolvervel_wdls.hpp" #include "utilities/svd_eigen_HH.hpp" -namespace KDL { - using namespace std; - +namespace KDL { TreeIkSolverVel_wdls::TreeIkSolverVel_wdls(const Tree& tree_in, const std::vector& endpoints) : tree(tree_in), jnttojacsolver(tree), - J(MatrixXd::Zero(6 * endpoints.size(), tree.getNrOfJoints())), - Wy(MatrixXd::Identity(J.rows(),J.rows())), - Wq(MatrixXd::Identity(J.cols(),J.cols())), + J(Eigen::MatrixXd::Zero(6 * endpoints.size(), tree.getNrOfJoints())), + Wy(Eigen::MatrixXd::Identity(J.rows(),J.rows())), + Wq(Eigen::MatrixXd::Identity(J.cols(),J.cols())), J_Wq(J.rows(),J.cols()),Wy_J_Wq(J.rows(),J.cols()), - U(MatrixXd::Identity(J.rows(),J.cols())), - V(MatrixXd::Identity(J.cols(),J.cols())), + U(Eigen::MatrixXd::Identity(J.rows(),J.cols())), + V(Eigen::MatrixXd::Identity(J.cols(),J.cols())), Wy_U(J.rows(),J.rows()),Wq_V(J.cols(),J.cols()), - t(VectorXd::Zero(J.rows())), Wy_t(VectorXd::Zero(J.rows())), - qdot(VectorXd::Zero(J.cols())), - tmp(VectorXd::Zero(J.cols())),S(VectorXd::Zero(J.cols())), + t(Eigen::VectorXd::Zero(J.rows())), Wy_t(Eigen::VectorXd::Zero(J.rows())), + qdot(Eigen::VectorXd::Zero(J.cols())), + tmp(Eigen::VectorXd::Zero(J.cols())),S(Eigen::VectorXd::Zero(J.cols())), lambda(0) { @@ -35,11 +33,11 @@ namespace KDL { TreeIkSolverVel_wdls::~TreeIkSolverVel_wdls() { } - void TreeIkSolverVel_wdls::setWeightJS(const MatrixXd& Mq) { + void TreeIkSolverVel_wdls::setWeightJS(const Eigen::MatrixXd& Mq) { Wq = Mq; } - void TreeIkSolverVel_wdls::setWeightTS(const MatrixXd& Mx) { + void TreeIkSolverVel_wdls::setWeightTS(const Eigen::MatrixXd& Mx) { Wy = Mx; } diff --git a/orocos_kdl/src/treeiksolvervel_wdls.hpp b/orocos_kdl/src/treeiksolvervel_wdls.hpp index 6f81615da..d6873d1b7 100644 --- a/orocos_kdl/src/treeiksolvervel_wdls.hpp +++ b/orocos_kdl/src/treeiksolvervel_wdls.hpp @@ -14,8 +14,6 @@ namespace KDL { - using namespace Eigen; - class TreeIkSolverVel_wdls: public TreeIkSolverVel { public: static const int E_SVD_FAILED = -100; //! Child SVD failed @@ -47,8 +45,8 @@ namespace KDL { * it gets an infinite weight in the norm computation. For * more detailed explanation : vincent.padois@upmc.fr */ - void setWeightJS(const MatrixXd& Mq); - const MatrixXd& getWeightJS() const {return Wq;} + void setWeightJS(const Eigen::MatrixXd& Mq); + const Eigen::MatrixXd& getWeightJS() const {return Wq;} /* * Set the task space weighting matrix @@ -73,8 +71,8 @@ namespace KDL { * weighted norm sqrt(|x_dot-Jq_dot|'*(M_x^2)*|x_dot-Jq_dot|). * For more detailed explanation : vincent.padois@upmc.fr */ - void setWeightTS(const MatrixXd& Mx); - const MatrixXd& getWeightTS() const {return Wy;} + void setWeightTS(const Eigen::MatrixXd& Mx); + const Eigen::MatrixXd& getWeightTS() const {return Wy;} void setLambda(const double& lambda); double getLambda () const {return lambda;} @@ -84,8 +82,8 @@ namespace KDL { TreeJntToJacSolver jnttojacsolver; Jacobians jacobians; - MatrixXd J, Wy, Wq, J_Wq, Wy_J_Wq, U, V, Wy_U, Wq_V; - VectorXd t, Wy_t, qdot, tmp, S; + Eigen::MatrixXd J, Wy, Wq, J_Wq, Wy_J_Wq, U, V, Wy_U, Wq_V; + Eigen::VectorXd t, Wy_t, qdot, tmp, S; double lambda; }; diff --git a/orocos_kdl/src/treejnttojacsolver.cpp b/orocos_kdl/src/treejnttojacsolver.cpp index 9b5ff856a..0afb4d1c5 100644 --- a/orocos_kdl/src/treejnttojacsolver.cpp +++ b/orocos_kdl/src/treejnttojacsolver.cpp @@ -47,7 +47,7 @@ int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, const std: T_total = T_local * T_total; //get the twist of the segment: - if (GetTreeElementSegment(it->second).getJoint().getType() != Joint::None) { + if (GetTreeElementSegment(it->second).getJoint().getType() != Joint::Fixed) { Twist t_local = GetTreeElementSegment(it->second).twist(q_in(q_nr), 1.0); //transform the endpoint of the local twist to the global endpoint: t_local = t_local.RefPoint(T_total.p - T_local.p); diff --git a/orocos_kdl/src/utilities/error_stack.cxx b/orocos_kdl/src/utilities/error_stack.cxx index 78c5027a9..c4837e6e1 100644 --- a/orocos_kdl/src/utilities/error_stack.cxx +++ b/orocos_kdl/src/utilities/error_stack.cxx @@ -57,7 +57,11 @@ void IOTracePopStr(char* buffer,int size) { *buffer = 0; return; } +#if defined(_WIN32) + strncpy_s(buffer,size,errorstack.top().c_str(),size); +#else strncpy(buffer,errorstack.top().c_str(),size); +#endif buffer[size - 1] = '\0'; errorstack.pop(); } diff --git a/orocos_kdl/src/utilities/hash_combine.h b/orocos_kdl/src/utilities/hash_combine.h new file mode 100644 index 000000000..978903a2a --- /dev/null +++ b/orocos_kdl/src/utilities/hash_combine.h @@ -0,0 +1,26 @@ +#ifndef KDL_HASH_COMBINE_H_ +#define KDL_HASH_COMBINE_H_ + +#include + +namespace KDL +{ + +/** + * @brief Combine hash of object \p v to the \p seed + * @param seed Seed to append the hash of \p v + * @param v Object of which the hash should be appended to the seed + * + * Inspired by: + * @link https://github.com/boostorg/multiprecision/blob/boost-1.79.0/include/boost/multiprecision/detail/hash.hpp#L35-L41 + */ +template +inline void hash_combine(std::size_t& seed, const T& v) +{ + std::hash hasher; + seed ^= hasher(v) + 0x9e3779b9 + (seed << 6) + (seed >> 2); +} + +} + +#endif diff --git a/orocos_kdl/src/utilities/ldl_solver_eigen.cpp b/orocos_kdl/src/utilities/ldl_solver_eigen.cpp new file mode 100644 index 000000000..3f47afdc2 --- /dev/null +++ b/orocos_kdl/src/utilities/ldl_solver_eigen.cpp @@ -0,0 +1,82 @@ +// Copyright (C) 2018 Craig Carignan + +// Version: 1.0 +// Author: Craig Carignan +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "ldl_solver_eigen.hpp" + +namespace KDL{ + + int ldl_solver_eigen(const Eigen::MatrixXd& A, const Eigen::VectorXd& v, Eigen::MatrixXd& L, Eigen::VectorXd& D, Eigen::VectorXd& vtmp, Eigen::VectorXd& q) + { + const int n = static_cast(A.rows()); + int error=SolverI::E_NOERROR; + + //Check sizes + if(A.cols()!=n || v.rows()!=n || L.rows()!=n || L.cols()!=n || D.rows()!=n || vtmp.rows()!=n || q.rows()!=n) + return (error = SolverI::E_SIZE_MISMATCH); + + for(int i=0;i0) { + for(int j=0;j<=i-1;++j) + D(i) -= D(j)*L(i,j)*L(i,j); + } + for(int j=1;ji) { + L(j,i)=A(i,j)/D(i); + if(i>0) { + for(int k=0;k<=i-1;++k) + L(j,i) -= L(j,k)*L(i,k)*D(k)/D(i); + } + } + } + } + for(int i=0;i0) { + for(int j=0;j<=i-1;++j) + vtmp(i) -= L(i,j)*vtmp(j); + } + } + for(int i=n-1;i>=0;--i) { + q(i)=vtmp(i)/D(i); + if(i 1 ) + { + for(int i=0;i + +// Version: 1.0 +// Author: Craig Carignan +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +// Inverse of a positive definite symmetric matrix times a vector +// based on LDL^T Decomposition +#ifndef LDL_SOLVER_EIGEN_HPP +#define LDL_SOLVER_EIGEN_HPP + + +#include +#include "../solveri.hpp" + +namespace KDL +{ + /** + * \brief Solves the system of equations Aq = v for q via LDL decomposition, + * where A is a square positive definite matrix + * + * The algorithm factor A into the product of three matrices LDL^T, where L + * is a lower triangular matrix and D is a diagonal matrix. This allows q + * to be computed without explicitly inverting A. Note that the LDL decomposition + * is a variant of the classical Cholesky Decomposition that does not require + * the computation of square roots. + * Input parameters: + * @param A matrix(nxn) + * @param v vector n + * @param vtmp vector n [temp variable] + * Output parameters: + * @param L matrix(nxn) + * @param D vector n + * @param q vector n + * @return 0 if successful, E_SIZE_MISMATCH if dimensions do not match + * References: + * https://en.wikipedia.org/wiki/Cholesky_decomposition + */ + int ldl_solver_eigen(const Eigen::MatrixXd& A, const Eigen::VectorXd& v, Eigen::MatrixXd& L, Eigen::VectorXd& D, Eigen::VectorXd& vtmp, Eigen::VectorXd& q); +} +#endif diff --git a/orocos_kdl/src/utilities/rall1d.h b/orocos_kdl/src/utilities/rall1d.h index 4b3ba1ac1..812c69fe3 100644 --- a/orocos_kdl/src/utilities/rall1d.h +++ b/orocos_kdl/src/utilities/rall1d.h @@ -56,7 +56,7 @@ class Rall1d T t; //!< value V grad; //!< gradient public : - INLINE Rall1d() {} + INLINE Rall1d():t(),grad() {}; T value() const { return t; @@ -471,6 +471,22 @@ INLINE bool Equal(const Rall1d& y,const Rall1d& x,double eps=epsi return (Equal(x.t,y.t,eps)&&Equal(x.grad,y.grad,eps)); } +template +INLINE bool operator==(const Rall1d& y,const Rall1d& x) +{ +#ifdef KDL_USE_EQUAL + return Equal(y, x); +#else + return (x.t == y.t && x.grad == y.grad); +#endif +} + +template +INLINE bool operator!=(const Rall1d& y,const Rall1d& x) +{ + return !operator==(y, x); +} + } diff --git a/orocos_kdl/src/utilities/rall1d_io.h b/orocos_kdl/src/utilities/rall1d_io.h index d69b26177..ac2115904 100644 --- a/orocos_kdl/src/utilities/rall1d_io.h +++ b/orocos_kdl/src/utilities/rall1d_io.h @@ -18,6 +18,7 @@ #ifndef Rall_IO_H #define Rall_IO_H +#include #include "utility_io.h" #include "rall1d.h" @@ -26,7 +27,7 @@ namespace KDL { template inline std::ostream& operator << (std::ostream& os,const Rall1d& r) { - os << "Rall1d(" << r.t <<"," << r.grad <<")"; + os << "Rall1d<" << typeid(T).name() << ", "<< typeid(V).name() << ", " << typeid(S).name() << ">(" << r.t <<"," << r.grad <<")"; return os; } diff --git a/orocos_kdl/src/utilities/rall2d.h b/orocos_kdl/src/utilities/rall2d.h index 811bad553..fd6f6f965 100644 --- a/orocos_kdl/src/utilities/rall2d.h +++ b/orocos_kdl/src/utilities/rall2d.h @@ -59,7 +59,7 @@ class Rall2d V dd; //!< 2nd derivative public : // = Constructors - INLINE Rall2d() {} + INLINE Rall2d():t(),d(),dd() {}; explicit INLINE Rall2d(typename TI::Arg c) {t=c;SetToZero(d);SetToZero(dd);} @@ -531,6 +531,24 @@ INLINE bool Equal(const Rall2d& y,const Rall2d& x,double eps=epsi ); } +template +INLINE bool operator==(const Rall2d& y,const Rall2d& x) +{ +#ifdef KDL_USE_EQUAL + return Equal(y, x); +#else + return (x.t == y.t && + x.d == y.d && + x.dd == y.dd); +#endif + +} + +template +INLINE bool operator!=(const Rall2d& y,const Rall2d& x) +{ + return !operator==(y, x); +} } diff --git a/orocos_kdl/src/utilities/rall2d_io.h b/orocos_kdl/src/utilities/rall2d_io.h index 8870243a5..5ea145cd2 100644 --- a/orocos_kdl/src/utilities/rall2d_io.h +++ b/orocos_kdl/src/utilities/rall2d_io.h @@ -19,7 +19,7 @@ #define Rall2d_IO_H - +#include #include "utility_io.h" #include "rall2d.h" @@ -28,7 +28,7 @@ namespace KDL { template std::ostream& operator << (std::ostream& os,const Rall2d& r) { - os << "Rall2d(" << r.t <<"," << r.d <<","<(" << r.t <<"," << r.d <<","< 199711L) +#include +#else +#include +#endif + +namespace KDL { + +template class scoped_ptr; + +template void swap(scoped_ptr&, scoped_ptr&); + +template +class scoped_ptr { + public: + scoped_ptr() : ptr_(0) { } + explicit scoped_ptr(T* p) : ptr_(p) { } + + ~scoped_ptr() { delete ptr_; } + + T* operator->() { return ptr_; } + const T* operator->() const { return ptr_; } + + T* get() const { return ptr_; } + + void reset(T* p = 0) { + T* old = ptr_; + ptr_ = p; + delete old; + } + + T* release() { + T* old = ptr_; + ptr_ = 0; + return old; + } + + friend void swap<>(scoped_ptr& a, scoped_ptr& b); + + private: + scoped_ptr(const scoped_ptr&); // not-copyable + scoped_ptr& operator=(const scoped_ptr&); // not-copyable + + T* ptr_; +}; + +template +void swap(scoped_ptr& a, scoped_ptr& b) { + using std::swap; + swap(a.ptr_, b.ptr_); +} + + +} // namespace KDL diff --git a/orocos_kdl/src/utilities/svd_HH.cpp b/orocos_kdl/src/utilities/svd_HH.cpp index 296565b30..b82467cd0 100644 --- a/orocos_kdl/src/utilities/svd_HH.cpp +++ b/orocos_kdl/src/utilities/svd_HH.cpp @@ -130,7 +130,7 @@ namespace KDL } maxarg1=anorm; maxarg2=(fabs(w(i))+fabs(tmp(i))); - anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2; + anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2; } /* Accumulation of right-hand transformations. */ for (i=cols-1;i>=0;i--) { diff --git a/orocos_kdl/src/utilities/svd_eigen_HH.cpp b/orocos_kdl/src/utilities/svd_eigen_HH.cpp index 36da50914..7c7bd3794 100644 --- a/orocos_kdl/src/utilities/svd_eigen_HH.cpp +++ b/orocos_kdl/src/utilities/svd_eigen_HH.cpp @@ -23,11 +23,11 @@ namespace KDL{ - int svd_eigen_HH(const MatrixXd& A,MatrixXd& U,VectorXd& S,MatrixXd& V,VectorXd& tmp,int maxiter,double epsilon) + int svd_eigen_HH(const Eigen::MatrixXd &A, Eigen::MatrixXd &U, Eigen::VectorXd &S, Eigen::MatrixXd &V, Eigen::VectorXd &tmp, int maxiter, double epsilon) { //get the rows/columns of the matrix - const int rows = A.rows(); - const int cols = A.cols(); + const int rows = static_cast(A.rows()); + const int cols = static_cast(A.cols()); U.setZero(); U.topLeftCorner(rows,cols)=A; @@ -53,14 +53,14 @@ namespace KDL{ s += U(k,i)*U(k,i); } f=U(i,i); // f is the diag elem - if (!(s>=0)) return -3; + if (!(s>=0)) return -3; g = -SIGN(sqrt(s),f); h=f*g-s; U(i,i)=f-g; for (j=ppi;j=0)) return -5; + if (!(s>=0)) return -5; g = -SIGN(sqrt(s),f); h=f*g-s; U(i,ppi)=f-g; - if (!(h!=0)) return -6; + if (!(h!=0)) return -6; for (k=ppi;k maxarg2 ? maxarg1 : maxarg2; + anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2; } /* Accumulation of right-hand transformations. */ for (i=cols-1;i>=0;i--) { if (iepsilon) { - if (!(U(i,ppi)!=0)) return -7; + if (!(U(i,ppi)!=0)) return -7; for (j=ppi;j #include -using namespace Eigen; - namespace KDL { inline double PYTHAG(double a,double b) { @@ -66,6 +64,6 @@ namespace KDL * * @return -2 if maxiter exceeded, 0 otherwise */ - int svd_eigen_HH(const MatrixXd& A,MatrixXd& U,VectorXd& S,MatrixXd& V,VectorXd& tmp,int maxiter=150,double epsilon=1e-300); + int svd_eigen_HH(const Eigen::MatrixXd& A,Eigen::MatrixXd& U,Eigen::VectorXd& S,Eigen::MatrixXd& V,Eigen::VectorXd& tmp,int maxiter=150,double epsilon=1e-300); } #endif diff --git a/orocos_kdl/src/utilities/svd_eigen_Macie.cpp b/orocos_kdl/src/utilities/svd_eigen_Macie.cpp new file mode 100644 index 000000000..c0fce479e --- /dev/null +++ b/orocos_kdl/src/utilities/svd_eigen_Macie.cpp @@ -0,0 +1,162 @@ +// Copyright (C) 2007 Ruben Smits + +// Version: 1.0 +// Author: Ruben Smits +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "svd_eigen_Macie.hpp" + +namespace KDL{ + int svd_eigen_Macie(const Eigen::MatrixXd& A,Eigen::MatrixXd& U,Eigen::VectorXd& S, Eigen::MatrixXd& V, + Eigen::MatrixXd& B, Eigen::VectorXd& tempi, + double threshold,bool toggle) + { + bool rotate = true; + unsigned int sweeps=0; + unsigned int rotations=0; + if(toggle){ + //Calculate B from new A and previous V + B=A.lazyProduct(V); + while(rotate){ + rotate=false; + rotations=0; + //Perform rotations between columns of B + for(unsigned int i=0;i=0){ + cos=sqrt((c+q)/(2*c)); + sin=p/(c*cos); + }else{ + if(p<0) + sin=-sqrt((c-q)/(2*c)); + else + sin=sqrt((c-q)/(2*c)); + cos=p/(c*sin); + } + + //Apply plane rotation to columns of B + tempi = cos*B.col(i) + sin*B.col(j); + B.col(j) = - sin*B.col(i) + cos*B.col(j); + B.col(i) = tempi; + + //Apply plane rotation to columns of V + tempi.head(V.rows()) = cos*V.col(i) + sin*V.col(j); + V.col(j) = - sin*V.col(i) + cos*V.col(j); + V.col(i) = tempi.head(V.rows()); + + rotate=true; + } + } + //Only calculate new U and S if there were any rotations + if(rotations!=0){ + for(unsigned int i=0;i=0){ + cos=sqrt((c+q)/(2*c)); + sin=p/(c*cos); + }else{ + if(p<0) + sin=-sqrt((c-q)/(2*c)); + else + sin=sqrt((c-q)/(2*c)); + cos=p/(c*sin); + } + + //Apply plane rotation to rows of B + tempi.head(B.cols()) = cos*B.row(i) + sin*B.row(j); + B.row(j) = - sin*B.row(i) + cos*B.row(j); + B.row(i) = tempi.head(B.cols()); + + + //Apply plane rotation to rows of U + tempi.head(U.rows()) = cos*U.col(i) + sin*U.col(j); + U.col(j) = - sin*U.col(i) + cos*U.col(j); + U.col(i) = tempi.head(U.rows()); + + rotate=true; + } + } + + //Only calculate new U and S if there were any rotations + if(rotations!=0){ + for(unsigned int i=0;i -using namespace Eigen; namespace KDL @@ -56,143 +55,9 @@ namespace KDL * \param toggle [INPUT] toggle this boolean variable on each call of this routine. * \return number of sweeps. */ - int svd_eigen_Macie(const MatrixXd& A,MatrixXd& U,VectorXd& S, MatrixXd& V, - MatrixXd& B, VectorXd& tempi, - double threshold,bool toggle) - { - bool rotate = true; - unsigned int sweeps=0; - unsigned int rotations=0; - if(toggle){ - //Calculate B from new A and previous V - B=A.lazyProduct(V); - while(rotate){ - rotate=false; - rotations=0; - //Perform rotations between columns of B - for(unsigned int i=0;i=0){ - cos=sqrt((c+q)/(2*c)); - sin=p/(c*cos); - }else{ - if(p<0) - sin=-sqrt((c-q)/(2*c)); - else - sin=sqrt((c-q)/(2*c)); - cos=p/(c*sin); - } - - //Apply plane rotation to columns of B - tempi = cos*B.col(i) + sin*B.col(j); - B.col(j) = - sin*B.col(i) + cos*B.col(j); - B.col(i) = tempi; - - //Apply plane rotation to columns of V - tempi.head(V.rows()) = cos*V.col(i) + sin*V.col(j); - V.col(j) = - sin*V.col(i) + cos*V.col(j); - V.col(i) = tempi.head(V.rows()); - - rotate=true; - } - } - //Only calculate new U and S if there were any rotations - if(rotations!=0){ - for(unsigned int i=0;i=0){ - cos=sqrt((c+q)/(2*c)); - sin=p/(c*cos); - }else{ - if(p<0) - sin=-sqrt((c-q)/(2*c)); - else - sin=sqrt((c-q)/(2*c)); - cos=p/(c*sin); - } - - //Apply plane rotation to rows of B - tempi.head(B.cols()) = cos*B.row(i) + sin*B.row(j); - B.row(j) = - sin*B.row(i) + cos*B.row(j); - B.row(i) = tempi.head(B.cols()); - - - //Apply plane rotation to rows of U - tempi.head(U.rows()) = cos*U.col(i) + sin*U.col(j); - U.col(j) = - sin*U.col(i) + cos*U.col(j); - U.col(i) = tempi.head(U.rows()); - - rotate=true; - } - } - - //Only calculate new U and S if there were any rotations - if(rotations!=0){ - for(unsigned int i=0;i 199711L) +using std::isnan; +#endif /** * Auxiliary class for argument types (Trait-template class ) @@ -179,6 +179,12 @@ extern int MAXLENFILENAME; //! the value of pi extern const double PI; +//! the value of pi/2 +extern const double PI_2; + +//! the value of pi/4 +extern const double PI_4; + //! the value pi/180 extern const double deg2rad; diff --git a/orocos_kdl/src/velocityprofile.cpp b/orocos_kdl/src/velocityprofile.cpp index f83fe4e3f..dec245384 100644 --- a/orocos_kdl/src/velocityprofile.cpp +++ b/orocos_kdl/src/velocityprofile.cpp @@ -50,9 +50,7 @@ namespace KDL { -using namespace std; - -VelocityProfile* VelocityProfile::Read(istream& is) { +VelocityProfile* VelocityProfile::Read(std::istream& is) { IOTrace("VelocityProfile::Read"); char storage[25]; EatWord(is,"[",storage,sizeof(storage)); @@ -95,6 +93,4 @@ VelocityProfile* VelocityProfile::Read(istream& is) { return 0; } - - } diff --git a/orocos_kdl/src/velocityprofile_trap.cpp b/orocos_kdl/src/velocityprofile_trap.cpp index 798caa917..1bee674e5 100644 --- a/orocos_kdl/src/velocityprofile_trap.cpp +++ b/orocos_kdl/src/velocityprofile_trap.cpp @@ -91,12 +91,12 @@ void VelocityProfile_Trap::SetProfile(double pos1,double pos2) { void VelocityProfile_Trap::SetProfileDuration( double pos1,double pos2,double newduration) { // duration should be longer than originally planned duration - // Fastest : + // Fastest : SetProfile(pos1,pos2); - // Must be Slower : + // Must be Slower : double factor = duration/newduration; - if (factor > 1) - return; // do not exceed max + if (factor > 1) + return; // do not exceed max a2*=factor; a3*=factor*factor; b2*=factor; @@ -110,9 +110,9 @@ void VelocityProfile_Trap::SetProfileDuration( void VelocityProfile_Trap::SetProfileVelocity( double pos1,double pos2,double newvelocity) { - // Max velocity + // Max velocity SetProfile(pos1,pos2); - // Must be Slower : + // Must be Slower : double factor = newvelocity; // valid = [KDL::epsilon, 1.0] if (1.0 < factor) factor = 1.0; if (KDL::epsilon > factor) factor = KDL::epsilon; @@ -129,7 +129,7 @@ void VelocityProfile_Trap::SetProfileVelocity( void VelocityProfile_Trap::SetMax(double _maxvel,double _maxacc) { - maxvel = _maxvel; maxacc = _maxacc; + maxvel = _maxvel; maxacc = _maxacc; } double VelocityProfile_Trap::Duration() const { @@ -178,8 +178,8 @@ double VelocityProfile_Trap::Acc(double time) const { } VelocityProfile* VelocityProfile_Trap::Clone() const { - VelocityProfile_Trap* res = new VelocityProfile_Trap(maxvel,maxacc); - res->SetProfileDuration( this->startpos, this->endpos, this->duration ); + VelocityProfile_Trap* res = new VelocityProfile_Trap(maxvel,maxacc); + res->SetProfileDuration( this->startpos, this->endpos, this->duration ); return res; } diff --git a/orocos_kdl/src/velocityprofile_traphalf.cpp b/orocos_kdl/src/velocityprofile_traphalf.cpp index 80b63e0d6..05ff180bc 100644 --- a/orocos_kdl/src/velocityprofile_traphalf.cpp +++ b/orocos_kdl/src/velocityprofile_traphalf.cpp @@ -52,7 +52,7 @@ VelocityProfile_TrapHalf::VelocityProfile_TrapHalf(double _maxvel,double _maxacc void VelocityProfile_TrapHalf::SetMax(double _maxvel,double _maxacc, bool _starting) { - maxvel = _maxvel; maxacc = _maxacc; starting = _starting; + maxvel = _maxvel; maxacc = _maxacc; starting = _starting; } void VelocityProfile_TrapHalf::PlanProfile1(double v,double a) { @@ -100,11 +100,11 @@ void VelocityProfile_TrapHalf::SetProfile(double pos1,double pos2) { void VelocityProfile_TrapHalf::SetProfileDuration( double pos1,double pos2,double newduration) { - SetProfile(pos1,pos2); - double factor = duration/newduration; + SetProfile(pos1,pos2); + double factor = duration/newduration; - if ( factor > 1 ) - return; + if ( factor > 1 ) + return; double s = sign(endpos-startpos); double tmp = 2.0*s*(endpos-startpos)/maxvel; @@ -183,8 +183,8 @@ double VelocityProfile_TrapHalf::Acc(double time) const { } VelocityProfile* VelocityProfile_TrapHalf::Clone() const { - VelocityProfile_TrapHalf* res = new VelocityProfile_TrapHalf(maxvel,maxacc, starting); - res->SetProfileDuration( this->startpos, this->endpos, this->duration ); + VelocityProfile_TrapHalf* res = new VelocityProfile_TrapHalf(maxvel,maxacc, starting); + res->SetProfileDuration( this->startpos, this->endpos, this->duration ); return res; } diff --git a/orocos_kdl/tests/CMakeLists.txt b/orocos_kdl/tests/CMakeLists.txt index 06382cdb4..ed045b2fc 100644 --- a/orocos_kdl/tests/CMakeLists.txt +++ b/orocos_kdl/tests/CMakeLists.txt @@ -8,80 +8,64 @@ IF(ENABLE_TESTS) SET(TESTNAME "framestest") SET_TARGET_PROPERTIES( framestest PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ") - ADD_TEST(framestest framestest) - + ADD_TEST(NAME framestest COMMAND framestest) + ADD_EXECUTABLE(kinfamtest kinfamtest.cpp test-runner.cpp) TARGET_LINK_LIBRARIES(kinfamtest orocos-kdl ${CPPUNIT}) SET(TESTNAME "kinfamtest") SET_TARGET_PROPERTIES( kinfamtest PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS}") - ADD_TEST(kinfamtest kinfamtest) + ADD_TEST(NAME kinfamtest COMMAND kinfamtest) ADD_EXECUTABLE(solvertest solvertest.cpp test-runner.cpp) TARGET_LINK_LIBRARIES(solvertest orocos-kdl ${CPPUNIT}) SET(TESTNAME "solvertest") SET_TARGET_PROPERTIES( solvertest PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ") - ADD_TEST(solvertest solvertest) + ADD_TEST(NAME solvertest COMMAND solvertest) ADD_EXECUTABLE(inertiatest inertiatest.cpp test-runner.cpp) TARGET_LINK_LIBRARIES(inertiatest orocos-kdl ${CPPUNIT}) SET(TESTNAME "inertiatest") SET_TARGET_PROPERTIES( inertiatest PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ") - ADD_TEST(inertiatest inertiatest) + ADD_TEST(NAME inertiatest COMMAND inertiatest) ADD_EXECUTABLE(jacobiantest jacobiantest.cpp test-runner.cpp) SET(TESTNAME "jacobiantest") TARGET_LINK_LIBRARIES(jacobiantest orocos-kdl ${CPPUNIT}) SET_TARGET_PROPERTIES( jacobiantest PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ") - ADD_TEST(jacobiantest jacobiantest) + ADD_TEST(NAME jacobiantest COMMAND jacobiantest) ADD_EXECUTABLE(jacobiandottest jacobiandottest.cpp test-runner.cpp) SET(TESTNAME "jacobiandottest") TARGET_LINK_LIBRARIES(jacobiandottest orocos-kdl ${CPPUNIT}) SET_TARGET_PROPERTIES( jacobiandottest PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ") - ADD_TEST(jacobiandottest jacobiandottest) - + ADD_TEST(NAME jacobiandottest COMMAND jacobiandottest) + ADD_EXECUTABLE(velocityprofiletest velocityprofiletest.cpp test-runner.cpp) SET(TESTNAME "velocityprofiletest") TARGET_LINK_LIBRARIES(velocityprofiletest orocos-kdl ${CPPUNIT}) SET_TARGET_PROPERTIES( velocityprofiletest PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ") - ADD_TEST(velocityprofiletest velocityprofiletest) + ADD_TEST(NAME velocityprofiletest COMMAND velocityprofiletest) + + ADD_EXECUTABLE(treeinvdyntest treeinvdyntest.cpp test-runner.cpp) + SET(TESTNAME "treeinvdyntest") + TARGET_LINK_LIBRARIES(treeinvdyntest orocos-kdl ${CPPUNIT}) + SET_TARGET_PROPERTIES( treeinvdyntest PROPERTIES + COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ") + ADD_TEST(NAME treeinvdyntest COMMAND treeinvdyntest) # ADD_EXECUTABLE(rframestest rframestest.cpp) # TARGET_LINK_LIBRARIES(rframestest orocos-kdl) -# ADD_TEST(rframestest rframestest) +# ADD_TEST(NAME rframestest COMMAND rframestest) # # ADD_EXECUTABLE(rallnumbertest rallnumbertest.cpp) # TARGET_LINK_LIBRARIES(rallnumbertest orocos-kdl) -# ADD_TEST(rallnumbertest rallnumbertest) -# -# -# IF(OROCOS_PLUGIN) -# ADD_EXECUTABLE(toolkittest toolkittest.cpp) -# LINK_DIRECTORIES(${OROCOS_RTT_LINK_DIRS}) -# TARGET_LINK_LIBRARIES(toolkittest orocos-kdltk orocos-kdl ${OROCOS_RTT_LIBS}) -# ADD_TEST(toolkittest toolkittest) -# ENDIF(OROCOS_PLUGIN) -# -# IF(PYTHON_BINDINGS) -# CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/framestest.py -# ${CMAKE_CURRENT_BINARY_DIR}/framestest.py COPY_ONLY) -# CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/kinfamtest.py -# ${CMAKE_CURRENT_BINARY_DIR}/kinfamtest.py COPY_ONLY) -# CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/frameveltest.py -# ${CMAKE_CURRENT_BINARY_DIR}/frameveltest.py COPY_ONLY) -# CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/PyKDLtest.py -# ${CMAKE_CURRENT_BINARY_DIR}/PyKDLtest.py COPY_ONLY) -# ADD_TEST(PyKDLtest PyKDLtest.py) -# -# -# ENDIF(PYTHON_BINDINGS) - - ADD_CUSTOM_TARGET(check ctest -V WORKING_DIRECTORY ${PROJ_BINARY_DIR}/tests) -ENDIF(ENABLE_TESTS) +# ADD_TEST(NAME rallnumbertest COMMAND rallnumbertest) + ADD_CUSTOM_TARGET(check ctest -V WORKING_DIRECTORY ${PROJ_BINARY_DIR}/tests) +ENDIF(ENABLE_TESTS) diff --git a/orocos_kdl/tests/framestest.cpp b/orocos_kdl/tests/framestest.cpp index bb8c0efb0..2a3ab4a6e 100644 --- a/orocos_kdl/tests/framestest.cpp +++ b/orocos_kdl/tests/framestest.cpp @@ -1,6 +1,7 @@ -#include #include "framestest.hpp" #include +#include + CPPUNIT_TEST_SUITE_REGISTRATION( FramesTest ); using namespace KDL; @@ -20,6 +21,7 @@ void FramesTest::TestVector2(Vector& v) { CPPUNIT_ASSERT_EQUAL(v+v+v-2*v,v); v2=v; CPPUNIT_ASSERT_EQUAL(v,v2); + CPPUNIT_ASSERT_EQUAL(std::hash{}(v), std::hash{}(v2)); v2+=v; CPPUNIT_ASSERT_EQUAL(2*v,v2); v2-=v; @@ -33,6 +35,9 @@ void FramesTest::TestVector() { TestVector2(v); Vector v2(0,0,0); TestVector2(v2); + + CPPUNIT_ASSERT_EQUAL(std::hash{}(v), static_cast(3450679443808348711u)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(v2), static_cast(11093822414574u)); Vector nu(0,0,0); CPPUNIT_ASSERT_EQUAL(nu.Norm(),0.0); @@ -58,11 +63,12 @@ void FramesTest::TestTwist2(Twist& t) { CPPUNIT_ASSERT_EQUAL(t+t+t-2*t,t); t2=t; CPPUNIT_ASSERT_EQUAL(t,t2); + CPPUNIT_ASSERT_EQUAL(std::hash{}(t), std::hash{}(t2)); t2+=t; CPPUNIT_ASSERT_EQUAL(2*t,t2); t2-=t; CPPUNIT_ASSERT_EQUAL(t,t2); - t.ReverseSign(); + t2.ReverseSign(); CPPUNIT_ASSERT_EQUAL(t,-t2); } @@ -73,6 +79,9 @@ void FramesTest::TestTwist() { TestTwist2(t2); Twist t3(Vector(0,-9,-3),Vector(1,-2,-4)); TestTwist2(t3); + + CPPUNIT_ASSERT_EQUAL(std::hash{}(t3), static_cast(3373832976806748309u)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(t2), static_cast(730713428471863u)); } void FramesTest::TestWrench2(Wrench& w) { @@ -83,11 +92,12 @@ void FramesTest::TestWrench2(Wrench& w) { CPPUNIT_ASSERT_EQUAL(w+w+w-2*w,w); w2=w; CPPUNIT_ASSERT_EQUAL(w,w2); + CPPUNIT_ASSERT_EQUAL(std::hash{}(w), std::hash{}(w2)); w2+=w; CPPUNIT_ASSERT_EQUAL(2*w,w2); w2-=w; CPPUNIT_ASSERT_EQUAL(w,w2); - w.ReverseSign(); + w2.ReverseSign(); CPPUNIT_ASSERT_EQUAL(w,-w2); } @@ -97,7 +107,10 @@ void FramesTest::TestWrench() { Wrench w2(Vector(0,0,0),Vector(0,0,0)); TestWrench2(w2); Wrench w3(Vector(2,1,4),Vector(5,3,1)); - TestWrench2(w3); + TestWrench2(w3); + + CPPUNIT_ASSERT_EQUAL(std::hash{}(w3), static_cast(13897938943539516747u)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(w2), static_cast(730713428471863u)); } void FramesTest::TestRotation2(const Vector& v,double a,double b,double c) { @@ -121,6 +134,7 @@ void FramesTest::TestRotation2(const Vector& v,double a,double b,double c) { CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitY(),R.UnitZ()),0.0,epsilon); R2=R; CPPUNIT_ASSERT_EQUAL(R,R2); + CPPUNIT_ASSERT_EQUAL(std::hash{}(R), std::hash{}(R2)); CPPUNIT_ASSERT_DOUBLES_EQUAL((R*v).Norm(),v.Norm(),epsilon); CPPUNIT_ASSERT_EQUAL(R.Inverse(R*v),v); CPPUNIT_ASSERT_EQUAL(R.Inverse(R*t),t); @@ -237,37 +251,37 @@ void FramesTest::TestEuler() { TESTEULERZYX(0.1,0,0) TESTEULERZYX(0,0,0.3) TESTEULERZYX(0,0,0) - TESTEULERZYX(0.3,0.999*M_PI/2,0.1) - // if beta== +/- M_PI/2 => multiple solutions available, gamma will be chosen to be zero ! + TESTEULERZYX(0.3,0.999*PI_2,0.1) + // if beta== +/- PI/2 => multiple solutions available, gamma will be chosen to be zero ! // so we test with gamma==0 ! - TESTEULERZYX(0.3,0.9999999999*M_PI/2,0) - TESTEULERZYX(0.3,0.99999999*M_PI/2,0) - TESTEULERZYX(0.3,0.999999*M_PI/2,0) - TESTEULERZYX(0.3,0.9999*M_PI/2,0) - TESTEULERZYX(0.3,0.99*M_PI/2,0) - //TESTEULERZYX(0.1,-M_PI/2,0.3) - TESTEULERZYX(0,M_PI/2,0) - - TESTEULERZYX(0.3,-M_PI/2,0) - TESTEULERZYX(0.3,-0.9999999999*M_PI/2,0) - TESTEULERZYX(0.3,-0.99999999*M_PI/2,0) - TESTEULERZYX(0.3,-0.999999*M_PI/2,0) - TESTEULERZYX(0.3,-0.9999*M_PI/2,0) - TESTEULERZYX(0.3,-0.99*M_PI/2,0) - TESTEULERZYX(0,-M_PI/2,0) + TESTEULERZYX(0.3,0.9999999999*PI_2,0) + TESTEULERZYX(0.3,0.99999999*PI_2,0) + TESTEULERZYX(0.3,0.999999*PI_2,0) + TESTEULERZYX(0.3,0.9999*PI_2,0) + TESTEULERZYX(0.3,0.99*PI_2,0) + //TESTEULERZYX(0.1,-PI_2,0.3) + TESTEULERZYX(0,PI_2,0) + + TESTEULERZYX(0.3,-PI_2,0) + TESTEULERZYX(0.3,-0.9999999999*PI_2,0) + TESTEULERZYX(0.3,-0.99999999*PI_2,0) + TESTEULERZYX(0.3,-0.999999*PI_2,0) + TESTEULERZYX(0.3,-0.9999*PI_2,0) + TESTEULERZYX(0.3,-0.99*PI_2,0) + TESTEULERZYX(0,-PI_2,0) // extremes of the range: - TESTEULERZYX(M_PI,-M_PI/2,0) - TESTEULERZYX(-M_PI,-M_PI/2,0) - TESTEULERZYX(M_PI,1,0) - TESTEULERZYX(-M_PI,1,0) - //TESTEULERZYX(0,-M_PI/2,M_PI) gamma will be chosen zero - //TESTEULERZYX(0,M_PI/2,-M_PI) gamma will be chosen zero - TESTEULERZYX(0,1,M_PI) + TESTEULERZYX(PI,-PI_2,0) + TESTEULERZYX(-PI,-PI_2,0) + TESTEULERZYX(PI,1,0) + TESTEULERZYX(-PI,1,0) + //TESTEULERZYX(0,-PI_2,PI) gamma will be chosen zero + //TESTEULERZYX(0,PI_2,-PI) gamma will be chosen zero + TESTEULERZYX(0,1,PI) TESTEULERZYZ(0.1,0.2,0.3) TESTEULERZYZ(-0.1,0.2,0.3) - TESTEULERZYZ(0.1,0.9*M_PI,0.3) + TESTEULERZYZ(0.1,0.9*PI,0.3) TESTEULERZYZ(0.1,0.2,-0.3) TESTEULERZYZ(0,0,0) TESTEULERZYZ(0,0,0) @@ -277,22 +291,22 @@ void FramesTest::TestEuler() { TESTEULERZYZ(0.4,PI,0) TESTEULERZYZ(0,PI,0) TESTEULERZYZ(PI,PI,0) - TESTEULERZYX(0.3,M_PI/2,0) - TESTEULERZYZ(0.3,0.9999999999*M_PI/2,0) - TESTEULERZYZ(0.3,0.99999999*M_PI/2,0) - TESTEULERZYZ(0.3,0.999999*M_PI/2,0) - TESTEULERZYZ(0.3,0.9999*M_PI/2,0) - TESTEULERZYZ(0.3,0.99*M_PI/2,0) - - TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, M_PI-0.2, 0.3+M_PI); - TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, M_PI-0.2, 0.3-M_PI); - TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, M_PI-0.2, 0.3+M_PI); - TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, M_PI-0.2, 0.3-M_PI); - - TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, -0.2, 0.3+M_PI); - TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, -0.2, 0.3+M_PI); - TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, -0.2, 0.3-M_PI); - TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, -0.2, 0.3-M_PI); + TESTEULERZYX(0.3,PI_2,0) + TESTEULERZYZ(0.3,0.9999999999*PI_2,0) + TESTEULERZYZ(0.3,0.99999999*PI_2,0) + TESTEULERZYZ(0.3,0.999999*PI_2,0) + TESTEULERZYZ(0.3,0.9999*PI_2,0) + TESTEULERZYZ(0.3,0.99*PI_2,0) + + TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1+PI, PI-0.2, 0.3+PI); + TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1-PI, PI-0.2, 0.3-PI); + TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1-PI, PI-0.2, 0.3+PI); + TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1+PI, PI-0.2, 0.3-PI); + + TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1+PI, -0.2, 0.3+PI); + TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1-PI, -0.2, 0.3+PI); + TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1+PI, -0.2, 0.3-PI); + TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1-PI, -0.2, 0.3-PI); } void FramesTest::TestRangeArbitraryRotation(const std::string& msg, @@ -405,12 +419,14 @@ void FramesTest::TestRotation() { TestOneRotation("rot([-1,-1,-1],-180)", KDL::Rotation::Rot(KDL::Vector(-1,-1,-1),-180*deg2rad), 180*deg2rad, Vector(1,1,1)/sqrt(3.0)); TestOneRotation("rot([0.707107, 0, 0.707107", KDL::Rotation::RPY(-2.9811968953315162, -atan(1)*2, -0.1603957582582825), 180*deg2rad, Vector(0.707107,0,0.707107) ); + + CPPUNIT_ASSERT_EQUAL(std::hash{}(Rotation::Quaternion(1, 0, 0, 0)), static_cast(5526237894416316219u)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(Rotation()), static_cast(8386870752212395617u)); } void FramesTest::TestGetRotAngle() { - static const double pi = atan(1)*4; double roll = -2.9811968953315162; - double pitch = -pi/2; + double pitch = -PI_2; double yaw = -0.1603957582582825; // rpy -> rotation @@ -436,7 +452,7 @@ void FramesTest::TestGetRotAngle() { { Vector axis; double angle = KDL::Rotation(-1, 0, 0 + 1e-6, 0, 1, 0, 0, 0, -1 - 1e-6).GetRotAngle(axis); - CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE("rot(NON-ORTHOGONAL, PI)", M_PI, angle, epsilon); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE("rot(NON-ORTHOGONAL, PI)", PI, angle, epsilon); } // Tests to show that GetRotAngle does not work for an improper rotation matrix which has a determinant of -1; @@ -506,60 +522,107 @@ void FramesTest::TestQuaternion() { CPPUNIT_ASSERT_DOUBLES_EQUAL(w, w2, epsilon); } +void FramesTest::TestOneRotationDiff( + const std::string& msg, + const Rotation& R_a_b1, + const Rotation& R_a_b2, + const Vector& expectedDiff) { + CPPUNIT_ASSERT_EQUAL_MESSAGE(msg, diff(R_a_b1, R_a_b2), expectedDiff); + CPPUNIT_ASSERT_EQUAL_MESSAGE(msg, addDelta(R_a_b1, expectedDiff), R_a_b2); +} void FramesTest::TestRotationDiff() { - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(0*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(90*deg2rad)), Vector(M_PI/2,0,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(180*deg2rad)), Vector(M_PI,0,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(270*deg2rad)), Vector(-M_PI/2,0,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(360*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-360*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-270*deg2rad)), Vector(M_PI/2,0,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-180*deg2rad)), Vector(M_PI,0,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-90*deg2rad)), Vector(-M_PI/2,0,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-0*deg2rad)), Vector(0,0,0)); // no rotation - - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(0*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(90*deg2rad)), Vector(0,M_PI/2,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(180*deg2rad)), Vector(0,M_PI,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(270*deg2rad)), Vector(0,-M_PI/2,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(360*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-360*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-270*deg2rad)), Vector(0,M_PI/2,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-180*deg2rad)), Vector(0,M_PI,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-90*deg2rad)), Vector(0,-M_PI/2,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-0*deg2rad)), Vector(0,0,0)); // no rotation - - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(0*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(180*deg2rad)), Vector(0,0,M_PI)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(270*deg2rad)), Vector(0,0,-M_PI/2)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(360*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-360*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-270*deg2rad)), Vector(0,0,M_PI/2)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-180*deg2rad)), Vector(0,0,M_PI)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-90*deg2rad)), Vector(0,0,-M_PI/2)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-0*deg2rad)), Vector(0,0,0)); // no rotation - - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotY(90*deg2rad)), Vector(0,M_PI/2,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2)); - - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::Identity(), Rotation::RotX(90*deg2rad)), Vector(M_PI/2,0,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::Identity(), Rotation::RotY(90*deg2rad)), Vector(0,M_PI/2,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::Identity(), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2)); - - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RPY(+0*deg2rad,0,-90*deg2rad), - Rotation::RPY(-0*deg2rad,0,+90*deg2rad)), - Vector(0,0,M_PI)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RPY(+5*deg2rad,0,-0*deg2rad), - Rotation::RPY(-5*deg2rad,0,+0*deg2rad)), - Vector(-10*deg2rad,0,0)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(0*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(0*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(90*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(90*deg2rad), Vector(PI_2,0,0)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(180*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(180*deg2rad), Vector(PI,0,0)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(270*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(270*deg2rad), Vector(-PI_2,0,0)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(360*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(360*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-360*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(-360*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-270*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(-270*deg2rad), Vector(PI_2,0,0)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-180*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(-180*deg2rad), Vector(PI,0,0)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-90*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(-90*deg2rad), Vector(-PI_2,0,0)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-0*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(-0*deg2rad), Vector(0,0,0)); // no rotation + + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(0*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(0*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(90*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(90*deg2rad), Vector(0,PI_2,0)); + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(180*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(180*deg2rad), Vector(0,PI,0)); + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(270*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(270*deg2rad), Vector(0,-PI_2,0)); + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(360*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(360*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-360*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(-360*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-270*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(-270*deg2rad), Vector(0,PI_2,0)); + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-180*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(-180*deg2rad), Vector(0,PI,0)); + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-90*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(-90*deg2rad), Vector(0,-PI_2,0)); + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-0*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(-0*deg2rad), Vector(0,0,0)); // no rotation + + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(0*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(0*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(90*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(90*deg2rad), Vector(0,0,PI_2)); + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(180*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(180*deg2rad), Vector(0,0,PI)); + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(270*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(270*deg2rad), Vector(0,0,-PI_2)); + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(360*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(360*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-360*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(-360*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-270*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(-270*deg2rad), Vector(0,0,PI_2)); + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-180*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(-180*deg2rad), Vector(0,0,PI)); + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-90*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(-90*deg2rad), Vector(0,0,-PI_2)); + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-0*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(-0*deg2rad), Vector(0,0,0)); // no rotation + + TestOneRotationDiff("diff(RotX(0*deg2rad),RotZ(90*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotZ(90*deg2rad), Vector(0,0,PI_2)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotY(90*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotY(90*deg2rad), Vector(0,PI_2,0)); + TestOneRotationDiff("diff(RotY(0*deg2rad),RotZ(90*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotZ(90*deg2rad), Vector(0,0,PI_2)); + + TestOneRotationDiff("diff(Identity(),RotX(90*deg2rad))", + Rotation::Identity(), Rotation::RotX(90*deg2rad), Vector(PI_2,0,0)); + TestOneRotationDiff("diff(Identity(),RotY(0*deg2rad))", + Rotation::Identity(), Rotation::RotY(90*deg2rad), Vector(0,PI_2,0)); + TestOneRotationDiff("diff(Identity(),RotZ(0*deg2rad))", + Rotation::Identity(), Rotation::RotZ(90*deg2rad), Vector(0,0,PI_2)); + + TestOneRotationDiff("diff(Rotation::RPY(+0*deg2rad,0,-90*deg2rad),Rotation::RPY(-0*deg2rad,0,+90*deg2rad))", + Rotation::RPY(+0*deg2rad,0,-90*deg2rad), + Rotation::RPY(-0*deg2rad,0,+90*deg2rad), + Vector(0,0,PI)); + TestOneRotationDiff("diff(Rotation::RPY(+5*deg2rad,0,-0*deg2rad),Rotation::RPY(-5*deg2rad,0,+0*deg2rad))", + Rotation::RPY(+5*deg2rad,0,-0*deg2rad), + Rotation::RPY(-5*deg2rad,0,+0*deg2rad), + Vector(-10*deg2rad,0,0)); KDL::Rotation R1 = Rotation::RPY(+5*deg2rad,0,-90*deg2rad); - CPPUNIT_ASSERT_EQUAL(KDL::diff(R1, Rotation::RPY(-5*deg2rad,0,+90*deg2rad)), - R1*Vector(0, 0, 180*deg2rad)); + TestOneRotationDiff("diff(Rotation::RPY(+5*deg2rad,0,-90*deg2rad),Rotation::RPY(-5*deg2rad,0,+90*deg2rad))", + R1, Rotation::RPY(-5*deg2rad,0,+90*deg2rad), + R1*Vector(0, 0, 180*deg2rad)); } void FramesTest::TestFrame() { @@ -572,6 +635,7 @@ void FramesTest::TestFrame() { F = Frame(Rotation::EulerZYX(10*deg2rad,20*deg2rad,-10*deg2rad),Vector(4,-2,1)); F2=F ; CPPUNIT_ASSERT_EQUAL(F,F2); + CPPUNIT_ASSERT_EQUAL(std::hash{}(F), std::hash{}(F2)); CPPUNIT_ASSERT_EQUAL(F.Inverse(F*v),v); CPPUNIT_ASSERT_EQUAL(F.Inverse(F*t),t); CPPUNIT_ASSERT_EQUAL(F.Inverse(F*w),w); @@ -586,52 +650,83 @@ void FramesTest::TestFrame() { CPPUNIT_ASSERT_EQUAL(F*F.Inverse(),Frame::Identity()); CPPUNIT_ASSERT_EQUAL(F.Inverse()*F,Frame::Identity()); CPPUNIT_ASSERT_EQUAL(F.Inverse()*v,F.Inverse(v)); + + // Denavit-Hartenberg + Frame F_DH = Frame(Rotation(1, 0, 0, + 0, 0, -1, + 0, 1, 0), + Vector(0, 0, 0.36)); + CPPUNIT_ASSERT(Equal(Frame::DH(0.0, M_PI_2, 0.36, 0.0), F_DH)); + CPPUNIT_ASSERT(Equal(Frame().DH(0.0, M_PI_2, 0.36, 0.0), F_DH)); // Only for testing purposes, shouldn't use static function of instances + + Frame F_DH_Craig1989 = Frame(Rotation(1, 0, 0, + 0, 0, -1, + 0, 1, 0), + Vector(0, -0.36, 0)); + CPPUNIT_ASSERT(Equal(Frame::DH_Craig1989(0.0, M_PI_2, 0.36, 0.0), F_DH_Craig1989)); + CPPUNIT_ASSERT(Equal(Frame().DH_Craig1989(0.0, M_PI_2, 0.36, 0.0), F_DH_Craig1989)); // Only for testing purposes, shouldn't use static function of instances + + CPPUNIT_ASSERT_EQUAL(std::hash{}(F), static_cast(6112004106257185417u)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(Frame()), static_cast(8387572672274540708u)); +} + +JntArray CreateRandomJntArray(int size) +{ + JntArray j(size); + for (int i = 0; i(I0.data).isZero()); + CPPUNIT_ASSERT(Eigen::Map(I0.data).isZero()); RotationalInertia I1; - CPPUNIT_ASSERT(Map(I1.data).isZero()); - CPPUNIT_ASSERT(Map(I0.data).isApprox(Map(I1.data))); + CPPUNIT_ASSERT(Eigen::Map(I1.data).isZero()); + CPPUNIT_ASSERT((Eigen::Map(I0.data)-Eigen::Map(I1.data)).isZero()); RotationalInertia I2(1,2,3,4,5,6); //Check if copying works fine RotationalInertia I3=I2; - CPPUNIT_ASSERT(Map(I3.data).isApprox(Map(I2.data))); + CPPUNIT_ASSERT((Eigen::Map(I3.data)-Eigen::Map(I2.data)).isZero()); I2.data[0]=0.0; - CPPUNIT_ASSERT(!Map(I3.data).isApprox(Map(I2.data))); + CPPUNIT_ASSERT(!(Eigen::Map(I3.data)-Eigen::Map(I2.data)).isZero()); //Check if addition and multiplication works fine - Map(I0.data).setRandom(); + Eigen::Map(I0.data).setRandom(); I1=-2*I0; - CPPUNIT_ASSERT(!Map(I1.data).isZero()); + CPPUNIT_ASSERT(!Eigen::Map(I1.data).isZero()); I1=I1+I0+I0; - CPPUNIT_ASSERT(Map(I1.data).isZero()); + CPPUNIT_ASSERT(Eigen::Map(I1.data).isZero()); //Check if matrix is symmetric - CPPUNIT_ASSERT(Map(I2.data).isApprox(Map(I2.data).transpose())); + CPPUNIT_ASSERT((Eigen::Map(I2.data)-Eigen::Map(I2.data).transpose()).isZero()); //Check if angular momentum is correctly calculated: Vector omega; random(omega); Vector momentum=I2*omega; - - CPPUNIT_ASSERT(Map(momentum.data).isApprox(Map(I2.data)*Map(omega.data))); + CPPUNIT_ASSERT((Eigen::Map(momentum.data)-(Eigen::Map(I2.data)*Eigen::Map(omega.data))).isZero()); } void InertiaTest::TestRigidBodyInertia() { @@ -60,26 +58,26 @@ void InertiaTest::TestRigidBodyInertia() { RotationalInertia Ic; random(mass); random(c); - Matrix3d Ic_eig = Matrix3d::Random(); + Eigen::Matrix3d Ic_eig = Eigen::Matrix3d::Random(); //make it symmetric: - Map(Ic.data)=Ic_eig+Ic_eig.transpose(); + Eigen::Map(Ic.data)=Ic_eig+Ic_eig.transpose(); RigidBodyInertia I2(mass,c,Ic); //Check if construction works fine CPPUNIT_ASSERT_EQUAL(I2.getMass(),mass); - CPPUNIT_ASSERT(!Map(I2.getRotationalInertia().data).isZero()); + CPPUNIT_ASSERT(!Eigen::Map(I2.getRotationalInertia().data).isZero()); CPPUNIT_ASSERT_EQUAL(I2.getCOG(),c); - CPPUNIT_ASSERT(Map(I2.getRotationalInertia().data).isApprox(Map(Ic.data)-mass*(Map(c.data)*Map(c.data).transpose()-(Map(c.data).dot(Map(c.data))*Matrix3d::Identity())))); + CPPUNIT_ASSERT((Eigen::Map(I2.getRotationalInertia().data)-(Eigen::Map(Ic.data)-mass*(Eigen::Map(c.data)*Eigen::Map(c.data).transpose()-(Eigen::Map(c.data).dot(Eigen::Map(c.data))*Eigen::Matrix3d::Identity())))).isZero()); RigidBodyInertia I3=I2; //check if copying works fine CPPUNIT_ASSERT_EQUAL(I2.getMass(),I3.getMass()); CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I3.getCOG()); - CPPUNIT_ASSERT(Map(I2.getRotationalInertia().data).isApprox(Map(I3.getRotationalInertia().data))); + CPPUNIT_ASSERT((Eigen::Map(I2.getRotationalInertia().data)-Eigen::Map(I3.getRotationalInertia().data)).isZero()); //Check if multiplication and addition works fine RigidBodyInertia I4=-2*I2 +I3+I3; CPPUNIT_ASSERT_DOUBLES_EQUAL(I4.getMass(),0.0,epsilon); CPPUNIT_ASSERT_EQUAL(I4.getCOG(),Vector::Zero()); - CPPUNIT_ASSERT(Map(I4.getRotationalInertia().data).isZero()); + CPPUNIT_ASSERT(Eigen::Map(I4.getRotationalInertia().data).isZero()); //Check if transformations work fine //Check only rotation transformation @@ -90,13 +88,13 @@ void InertiaTest::TestRigidBodyInertia() { I4 = R.Inverse()*I3; CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(), epsilon); CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG()); - CPPUNIT_ASSERT(Map(I2.getRotationalInertia().data).isApprox(Map(I4.getRotationalInertia().data))); + CPPUNIT_ASSERT((Eigen::Map(I2.getRotationalInertia().data)-Eigen::Map(I4.getRotationalInertia().data)).isZero()); //rotation and total with p=0 Frame T(R); I4 = T*I2; CPPUNIT_ASSERT_DOUBLES_EQUAL(I3.getMass(),I4.getMass(), epsilon); CPPUNIT_ASSERT_EQUAL(I3.getCOG(),I4.getCOG()); - CPPUNIT_ASSERT(Map(I3.getRotationalInertia().data).isApprox(Map(I4.getRotationalInertia().data))); + CPPUNIT_ASSERT((Eigen::Map(I3.getRotationalInertia().data)-Eigen::Map(I4.getRotationalInertia().data)).isZero()); //Check only transformation Vector p; @@ -105,12 +103,12 @@ void InertiaTest::TestRigidBodyInertia() { I4 = I3.RefPoint(-p); CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(),epsilon); CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG()); - CPPUNIT_ASSERT(Map(I2.getRotationalInertia().data).isApprox(Map(I4.getRotationalInertia().data))); + CPPUNIT_ASSERT((Eigen::Map(I2.getRotationalInertia().data)-Eigen::Map(I4.getRotationalInertia().data)).isZero()); T=Frame(-p); I4 = T*I2; CPPUNIT_ASSERT_DOUBLES_EQUAL(I3.getMass(),I4.getMass(),epsilon); CPPUNIT_ASSERT_EQUAL(I3.getCOG(),I4.getCOG()); - CPPUNIT_ASSERT(Map(I3.getRotationalInertia().data).isApprox(Map(I4.getRotationalInertia().data))); + CPPUNIT_ASSERT((Eigen::Map(I3.getRotationalInertia().data)-Eigen::Map(I4.getRotationalInertia().data)).isZero()); random(T); I3=T*I2; @@ -126,7 +124,7 @@ void InertiaTest::TestRigidBodyInertia() { I4 = T.Inverse()*I3; CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(),epsilon); CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG()); - CPPUNIT_ASSERT(Map(I2.getRotationalInertia().data).isApprox(Map(I4.getRotationalInertia().data))); + CPPUNIT_ASSERT((Eigen::Map(I2.getRotationalInertia().data)-Eigen::Map(I4.getRotationalInertia().data)).isZero()); } void InertiaTest::TestArticulatedBodyInertia() { @@ -135,7 +133,7 @@ void InertiaTest::TestArticulatedBodyInertia() { RotationalInertia Ic; random(mass); random(c); - Matrix3d::Map(Ic.data).triangularView()= Matrix3d::Random().triangularView(); + Eigen::Matrix3d::Map(Ic.data).triangularView()= Eigen::Matrix3d::Random().triangularView(); RigidBodyInertia RBI2(mass,c,Ic); ArticulatedBodyInertia I2(RBI2); ArticulatedBodyInertia I1(mass,c,Ic); @@ -148,20 +146,20 @@ void InertiaTest::TestArticulatedBodyInertia() { CPPUNIT_ASSERT_EQUAL(I2.H,I1.H); CPPUNIT_ASSERT_EQUAL(I2.I,I1.I); - CPPUNIT_ASSERT_EQUAL(I2.M,(Matrix3d::Identity()*mass).eval()); + CPPUNIT_ASSERT_EQUAL(I2.M,(Eigen::Matrix3d::Identity()*mass).eval()); CPPUNIT_ASSERT(!I2.I.isZero()); - //CPPUNIT_ASSERT(I2.I.isApprox(Map(Ic.data)-mass*(Map(c.data)*Map(c.data).transpose()-(Map(c.data).dot(Map(c.data))*Matrix3d::Identity())),epsilon)); - //CPPUNIT_ASSERT(I2.H.isApprox(Map(c.data)*Map(c.data).transpose()-(Map(c.data).dot(Map(c.data))*Matrix3d::Identity()),epsilon)); +// CPPUNIT_ASSERT((I2.I-(Eigen::Map(Ic.data)-mass*(Eigen::Map(c.data)*Eigen::Map(c.data).transpose()-(Eigen::Map(c.data).dot(Eigen::Map(c.data))*Eigen::Matrix3d::Identity())))).isZero()); +// CPPUNIT_ASSERT((I2.H-(Eigen::Map(c.data)*Eigen::Map(c.data).transpose()-(Eigen::Map(c.data).dot(Eigen::Map(c.data))*Eigen::Matrix3d::Identity()))).isZero()); ArticulatedBodyInertia I3=I2; //check if copying works fine - CPPUNIT_ASSERT(I2.M.isApprox(I3.M)); - CPPUNIT_ASSERT(I2.H.isApprox(I3.H)); - CPPUNIT_ASSERT(I2.I.isApprox(I3.I)); + CPPUNIT_ASSERT((I2.M-I3.M).isZero()); + CPPUNIT_ASSERT((I2.H-I3.H).isZero()); + CPPUNIT_ASSERT((I2.I-I3.I).isZero()); //Check if multiplication and addition works fine ArticulatedBodyInertia I4=-2*I2 +I3+I3; - CPPUNIT_ASSERT(I4.M.isApprox(Matrix3d::Zero().eval())); - CPPUNIT_ASSERT(I4.H.isApprox(Matrix3d::Zero().eval())); - CPPUNIT_ASSERT(I4.I.isApprox(Matrix3d::Zero().eval())); + CPPUNIT_ASSERT(I4.M.isZero()); + CPPUNIT_ASSERT(I4.H.isZero()); + CPPUNIT_ASSERT(I4.I.isZero()); //Check if transformations work fine //Check only rotation transformation @@ -169,38 +167,38 @@ void InertiaTest::TestArticulatedBodyInertia() { Rotation R; random(R); I3 = R*I2; - Map E(R.data); - Matrix3d tmp = E.transpose()*I2.M*E; - CPPUNIT_ASSERT(I3.M.isApprox(tmp)); + Eigen::Map E(R.data); + Eigen::Matrix3d tmp = E.transpose()*I2.M*E; + CPPUNIT_ASSERT((I3.M-tmp).isZero()); tmp = E.transpose()*I2.H*E; - CPPUNIT_ASSERT(I3.H.isApprox(tmp)); + CPPUNIT_ASSERT((I3.H-tmp).isZero()); tmp = E.transpose()*I2.I*E; - CPPUNIT_ASSERT(I3.I.isApprox(tmp)); + CPPUNIT_ASSERT((I3.I-tmp).isZero()); I4 = R.Inverse()*I3; - CPPUNIT_ASSERT(I2.M.isApprox(I4.M)); - CPPUNIT_ASSERT(I2.H.isApprox(I4.H)); - CPPUNIT_ASSERT(I2.I.isApprox(I4.I)); + CPPUNIT_ASSERT((I2.M-I4.M).isZero()); + CPPUNIT_ASSERT((I2.H-I4.H).isZero()); + CPPUNIT_ASSERT((I2.I-I4.I).isZero()); //rotation and total with p=0 Frame T(R); I4 = T*I2; - CPPUNIT_ASSERT(I3.M.isApprox(I4.M)); - CPPUNIT_ASSERT(I3.H.isApprox(I4.H)); - CPPUNIT_ASSERT(I3.I.isApprox(I4.I)); + CPPUNIT_ASSERT((I3.M-I4.M).isZero()); + CPPUNIT_ASSERT((I3.H-I4.H).isZero()); + CPPUNIT_ASSERT((I3.I-I4.I).isZero()); //Check only transformation Vector p; random(p); I3 = I2.RefPoint(p); I4 = I3.RefPoint(-p); - CPPUNIT_ASSERT(I2.M.isApprox(I4.M)); - CPPUNIT_ASSERT(I2.H.isApprox(I4.H)); - CPPUNIT_ASSERT(I2.I.isApprox(I4.I)); + CPPUNIT_ASSERT((I2.M-I4.M).isZero()); + CPPUNIT_ASSERT((I2.H-I4.H).isZero()); + CPPUNIT_ASSERT((I2.I-I4.I).isZero()); T=Frame(-p); I4 = T*I2; - CPPUNIT_ASSERT(I3.M.isApprox(I4.M)); - CPPUNIT_ASSERT(I3.H.isApprox(I4.H)); - CPPUNIT_ASSERT(I3.I.isApprox(I4.I)); + CPPUNIT_ASSERT((I3.M-I4.M).isZero()); + CPPUNIT_ASSERT((I3.H-I4.H).isZero()); + CPPUNIT_ASSERT((I3.I-I4.I).isZero()); random(T); @@ -215,7 +213,7 @@ void InertiaTest::TestArticulatedBodyInertia() { random(T); I3 = T*I2; I4 = T.Inverse()*I3; - CPPUNIT_ASSERT(I2.M.isApprox(I4.M)); - CPPUNIT_ASSERT(I2.H.isApprox(I4.H)); - CPPUNIT_ASSERT(I2.I.isApprox(I4.I)); + CPPUNIT_ASSERT((I2.M-I4.M).isZero()); + CPPUNIT_ASSERT((I2.H-I4.H).isZero()); + CPPUNIT_ASSERT((I2.I-I4.I).isZero()); } diff --git a/orocos_kdl/tests/iotest.cpp b/orocos_kdl/tests/iotest.cpp index 637a46654..e682f99b6 100644 --- a/orocos_kdl/tests/iotest.cpp +++ b/orocos_kdl/tests/iotest.cpp @@ -10,25 +10,24 @@ #include #include -using namespace std; using namespace KDL; void test_io(KinematicFamily* kf) { // write a kf to the file tst.dat - ofstream os("tst.dat"); - os << kf << endl; - cout << kf << endl; + std::ofstream os("tst.dat"); + os << kf << std::endl; + std::cout << kf << std::endl; os.close(); // read a serial chain from the file tst.dat - ifstream is ("tst.dat"); + std::ifstream is ("tst.dat"); KinematicFamily* kf2; try { kf2 = readKinematicFamily(is); - cout << kf2 << endl; + std::cout << kf2 << std::endl; } catch (Error& err) { - IOTraceOutput(cerr); - cout << "ERROR : " << err.Description() << endl; + IOTraceOutput(std::cerr); + std::cout << "ERROR : " << err.Description() << std::endl; exit(-1); } @@ -40,10 +39,10 @@ void test_io(KinematicFamily* kf) { jnt2cartpos->evaluate(q);jnt2cartpos->getFrame(F1); jnt2cartpos2->evaluate(q);jnt2cartpos2->getFrame(F2); - cout << F1 << endl; - cout << F2 << endl; + std::cout << F1 << std::endl; + std::cout << F2 << std::endl; if (!Equal(F1,F2)) { - cerr << "Results are not equal" << endl; + std::cerr << "Results are not equal" << std::endl; exit(-1); } delete jnt2cartpos; diff --git a/orocos_kdl/tests/jacobiandottest.cpp b/orocos_kdl/tests/jacobiandottest.cpp index f453d7383..622581ffc 100644 --- a/orocos_kdl/tests/jacobiandottest.cpp +++ b/orocos_kdl/tests/jacobiandottest.cpp @@ -100,7 +100,7 @@ void changeRepresentation(Jacobian& J,const Frame& F_bs_ee,const int& representa // Ref Frame {ee}, Ref Point {ee} J.changeBase(F_bs_ee.M.Inverse()); break; - case ChainJntToJacDotSolver::INTERTIAL: + case ChainJntToJacDotSolver::INERTIAL: // Ref Frame {bs}, Ref Point {bs} J.changeRefPoint(-F_bs_ee.p); break; @@ -113,8 +113,8 @@ void Jdot_diff(const Jacobian& J_q, { assert(J_q.columns() == J_qdt.columns()); assert(J_q.columns() == Jdot.columns()); - for(int l=0;l<6;l++) - for(int c=0;cfirst); + subtree.addSegment(segment102, subtree.getSegment("Segment 5")->first); + std::cout << "Subtree (rooted at " << subroot << "):" << std::endl << tree2str(subtree) << std::endl; + CPPUNIT_ASSERT(!isSubtree(tree1.getSegment(subroot), subtree.getRootSegment())); + CPPUNIT_ASSERT(isSubtree(subtree.getRootSegment(), tree1.getSegment(subroot))); +} +//Utility to check if the set of segments in contained is a subset of container. +//In addition, all the children of a segment in contained must be present in +//container as children of the same segment. +bool isSubtree(const SegmentMap::const_iterator container, const SegmentMap::const_iterator contained) { + //Check that the container and contained point to the same link + if(container->first != contained->first) + return false; + //Check that each child of contained is a child of container + std::vector children = GetTreeElementChildren(contained->second); + for(unsigned int i=0; i < children.size(); i++) { + //look for a child of container whose name matches the one of the current child from contained + std::vector::const_iterator it = GetTreeElementChildren(container->second).begin(); + while(it != GetTreeElementChildren(container->second).end()) { + if((*it)->first == children[i]->first) + break; //segment found, exit the loop + it++; + } + if(it == GetTreeElementChildren(container->second).end()) + return false; //child of contained not found as child of container + //inspect recursively all the children + if(!isSubtree((*it), children[i])) + return false; + } + return true; +} diff --git a/orocos_kdl/tests/rallnumbertest.cpp b/orocos_kdl/tests/rallnumbertest.cpp index f28eff4d7..806caa9ac 100644 --- a/orocos_kdl/tests/rallnumbertest.cpp +++ b/orocos_kdl/tests/rallnumbertest.cpp @@ -26,7 +26,6 @@ using namespace KDL; -using namespace std; // Again something a little bit more complicated : autodiff to 2nd derivative with N variables template diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp index 76b5eb43e..c42c8b696 100644 --- a/orocos_kdl/tests/solvertest.cpp +++ b/orocos_kdl/tests/solvertest.cpp @@ -2,8 +2,9 @@ #include #include #include +#include #include - +#include CPPUNIT_TEST_SUITE_REGISTRATION( SolverTest ); @@ -106,19 +107,119 @@ void SolverTest::setUp() motomansia10.addSegment(Segment(Joint(Joint::None), Frame::DH_Craig1989(0.0, 0.0, 0.36, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0))); + Frame::DH_Craig1989(0.0, PI_2, 0.0, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, -M_PI_2, 0.36, 0.0))); + Frame::DH_Craig1989(0.0, -PI_2, 0.36, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0))); + Frame::DH_Craig1989(0.0, PI_2, 0.0, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, -M_PI_2, 0.36, 0.0))); + Frame::DH_Craig1989(0.0, -PI_2, 0.36, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0))); + Frame::DH_Craig1989(0.0, PI_2, 0.0, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, -M_PI_2, 0.0, 0.0))); + Frame::DH_Craig1989(0.0, -PI_2, 0.0, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), Frame(Rotation::Identity(),Vector(0.0,0.0,0.155)))); + + // Motoman SIA10 Chain with Mass Parameters (for forward dynamics tests) + + // effective motor inertia is included as joint inertia + static const double scale=1; + static const double offset=0; + static const double inertiamotorA=5.0; // effective motor inertia kg-m^2 + static const double inertiamotorB=3.0; // effective motor inertia kg-m^2 + static const double inertiamotorC=1.0; // effective motor inertia kg-m^2 + static const double damping=0; + static const double stiffness=0; + + // Segment Inertias + KDL::RigidBodyInertia inert1(15.0, KDL::Vector(0.0, -0.02, 0.0), // mass, CM + KDL::RotationalInertia(0.1, 0.05, 0.1, 0.0, 0.0, 0.0)); // inertia + KDL::RigidBodyInertia inert2(5.0, KDL::Vector(0.0, -0.02, -0.1), + KDL::RotationalInertia(0.01, 0.1, 0.1, 0.0, 0.0, 0.0)); + KDL::RigidBodyInertia inert3(5.0, KDL::Vector(0.0, -0.05, 0.02), + KDL::RotationalInertia(0.05, 0.01, 0.05, 0.0, 0.0, 0.0)); + KDL::RigidBodyInertia inert4(3.0, KDL::Vector(0.0, 0.02, -0.15), + KDL::RotationalInertia(0.1, 0.1, 0.01, 0.0, 0.0, 0.0)); + KDL::RigidBodyInertia inert5(3.0, KDL::Vector(0.0, -0.05, 0.01), + KDL::RotationalInertia(0.02, 0.01, 0.02, 0.0, 0.0, 0.0)); + KDL::RigidBodyInertia inert6(3.0, KDL::Vector(0.0, -0.01, -0.1), + KDL::RotationalInertia(0.1, 0.1, 0.01, 0.0, 0.0, 0.0)); + KDL::RigidBodyInertia inert7(1.0, KDL::Vector(0.0, 0.0, 0.05), + KDL::RotationalInertia(0.01, 0.01, 0.1, 0.0, 0.0, 0.0)); + + motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorA, damping, stiffness), + Frame::DH(0.0, PI_2, 0.36, 0.0), + inert1)); + motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorA, damping, stiffness), + Frame::DH(0.0, -PI_2, 0.0, 0.0), + inert2)); + motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorB, damping, stiffness), + Frame::DH(0.0, PI_2, 0.36, 0.0), + inert3)); + motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorB, damping, stiffness), + Frame::DH(0.0, -PI_2, 0.0, 0.0), + inert4)); + motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness), + Frame::DH(0.0, PI_2, 0.36, 0.0), + inert5)); + motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness), + Frame::DH(0.0, -PI_2, 0.0, 0.0), + inert6)); + motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness), + Frame::DH(0.0, 0.0, 0.0, 0.0), + inert7)); + motomansia10dyn.addSegment(Segment(Joint(Joint::None), + Frame(Rotation::Identity(),Vector(0.0,0.0,0.155)))); + + /** + * KUKA LWR 4 Chain with Dynamics Parameters (for Forward Dynamics and Vereshchagin solver tests) + * Necessary test model for the Vereshchagin solver: KDL's implementation of the Vereshchagin solver + * can only work with the robot chains that have equal number of joints and segments. + * Note: Joint effective inertia values in this model are closely aligned with the joint inertia + * of the real robot. These parameters are published in: Jubien, A., Gautier, M. and Janot, A., + * "Dynamic identification of the Kuka LWR robot using motor torques and joint torque sensors data.", + * IFAC Proceedings Volumes, 2014., 47(3), pp.8391-8396. + */ + kukaLWR.addSegment(Segment(Joint(Joint::RotZ, scale, offset, 3.19, damping, stiffness), + Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0), + Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2, + Vector::Zero(), + RotationalInertia(0.0,0.0,0.0115343,0.0,0.0,0.0)))); + + kukaLWR.addSegment(Segment(Joint(Joint::RotZ, scale, offset, 3.05, damping, stiffness), + Frame::DH_Craig1989(0.0, -1.5707963, 0.4, 0.0), + Frame::DH_Craig1989(0.0, -1.5707963, 0.4, 0.0).Inverse()*RigidBodyInertia(2, + Vector(0.0,-0.3120511,-0.0038871), + RotationalInertia(-0.5471572,-0.0000302,-0.5423253,0.0,0.0,0.0018828)))); + + kukaLWR.addSegment(Segment(Joint(Joint::RotZ, scale, offset, 1.98, damping, stiffness), + Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0), + Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2, + Vector(0.0,-0.0015515,0.0), + RotationalInertia(0.0063507,0.0,0.0107804,0.0,0.0,-0.0005147)))); + + kukaLWR.addSegment(Segment(Joint(Joint::RotZ, scale, offset, 2.05, damping, stiffness), + Frame::DH_Craig1989(0.0, 1.5707963, 0.39, 0.0), + Frame::DH_Craig1989(0.0, 1.5707963, 0.39, 0.0).Inverse()*RigidBodyInertia(2, + Vector(0.0,0.5216809,0.0), + RotationalInertia(-1.0436952,0.0,-1.0392780,0.0,0.0,0.0005324)))); + + kukaLWR.addSegment(Segment(Joint(Joint::RotZ, scale, offset, 0.787, damping, stiffness), + Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0), + Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2, + Vector(0.0,0.0119891,0.0), + RotationalInertia(0.0036654,0.0,0.0060429,0.0,0.0,0.0004226)))); + + kukaLWR.addSegment(Segment(Joint(Joint::RotZ, scale, offset, 0.391, damping, stiffness), + Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0), + Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2, + Vector(0.0,0.0080787,0.0), + RotationalInertia(0.0010431,0.0,0.0036376,0.0,0.0,0.0000101)))); + + kukaLWR.addSegment(Segment(Joint(Joint::RotZ, scale, offset, 0.394, damping, stiffness), + Frame::Identity(), + RigidBodyInertia(2, Vector::Zero(), RotationalInertia(0.000001,0.0,0.0001203,0.0,0.0,0.0)))); } void SolverTest::tearDown() @@ -146,7 +247,8 @@ void SolverTest::UpdateChainTest() ChainDynParam dynparam(chain2, Vector::Zero()); ChainIdSolver_RNE idsolver1(chain2, Vector::Zero()); unsigned int nr_of_constraints = 4; - ChainIdSolver_Vereshchagin idsolver2(chain2,Twist::Zero(),4); + ChainHdSolver_Vereshchagin hdsolver(chain2,Twist::Zero(),4); + ChainExternalWrenchEstimator extwrench_estimator(chain2,Vector::Zero(), 100.0, 30.0, 0.5); JntArray q_in(chain2.getNrOfJoints()); JntArray q_in2(chain2.getNrOfJoints()); @@ -158,12 +260,15 @@ void SolverTest::UpdateChainTest() } JntArray q_out(chain2.getNrOfJoints()); JntArray q_out2(chain2.getNrOfJoints()); + JntArray ff_tau(chain2.getNrOfJoints()); + JntArray constraint_tau(chain2.getNrOfJoints()); Jacobian jac(chain2.getNrOfJoints()); Frame T; Twist t; FrameVel T2; Wrenches wrenches(chain2.getNrOfSegments()); JntSpaceInertiaMatrix m(chain2.getNrOfJoints()); + Wrench wrench_out; Jacobian alpha(nr_of_constraints - 1); JntArray beta(nr_of_constraints - 1); @@ -185,10 +290,11 @@ void SolverTest::UpdateChainTest() CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolverpos2.CartToJnt(q_in,T,q_out)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolverpos3.CartToJnt(q_in,T,q_out)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2)); - CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2)); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToCoriolis(q_in, q_in2, q_out)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToGravity(q_in, q_out)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToMass(q_in, m)); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, extwrench_estimator.JntToExtWrench(q_in,q_in2,ff_tau,wrench_out)); jacsolver1.updateInternalDataStructures(); jacdotsolver1.updateInternalDataStructures(); @@ -200,8 +306,9 @@ void SolverTest::UpdateChainTest() iksolverpos2.updateInternalDataStructures(); iksolverpos3.updateInternalDataStructures(); idsolver1.updateInternalDataStructures(); - idsolver2.updateInternalDataStructures(); + hdsolver.updateInternalDataStructures(); dynparam.updateInternalDataStructures(); + extwrench_estimator.updateInternalDataStructures(); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH,fksolverpos.JntToCart(q_in, T, chain2.getNrOfSegments())); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH,fksolvervel.JntToCart(q_in3, T2, chain2.getNrOfSegments())); @@ -216,10 +323,11 @@ void SolverTest::UpdateChainTest() CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos2.CartToJnt(q_in,T,q_out)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos3.CartToJnt(q_in,T,q_out)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2)); - CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2)); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToCoriolis(q_in, q_in2, q_out)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToGravity(q_in, q_out)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToMass(q_in, m)); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, extwrench_estimator.JntToExtWrench(q_in,q_in2,ff_tau,wrench_out)); q_in.resize(chain2.getNrOfJoints()); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments())); @@ -233,24 +341,28 @@ void SolverTest::UpdateChainTest() CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos2.CartToJnt(q_in,T,q_out)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos3.CartToJnt(q_in,T,q_out)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2)); - CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2)); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToCoriolis(q_in, q_in2, q_out)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToGravity(q_in, q_out)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToMass(q_in, m)); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, extwrench_estimator.JntToExtWrench(q_in,q_in2,ff_tau,wrench_out)); q_in2.resize(chain2.getNrOfJoints()); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToCoriolis(q_in, q_in2, q_out)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2)); - CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2)); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau)); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, extwrench_estimator.JntToExtWrench(q_in,q_in2,ff_tau,wrench_out)); wrenches.resize(chain2.getNrOfSegments()); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2)); - CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2)); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau)); q_out2.resize(chain2.getNrOfSegments()); + ff_tau.resize(chain2.getNrOfSegments()); + constraint_tau.resize(chain2.getNrOfSegments()); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2)); - CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2)); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau)); alpha.resize(nr_of_constraints); - CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2)); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau)); beta.resize(nr_of_constraints); - CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2)); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau)); jac.resize(chain2.getNrOfJoints()); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments())); q_out.resize(chain2.getNrOfJoints()); @@ -271,10 +383,11 @@ void SolverTest::UpdateChainTest() CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos2.CartToJnt(q_in,T,q_out)); CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos3.CartToJnt(q_in,T,q_out)); CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2)); - CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2)); + CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau)); CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToCoriolis(q_in, q_in2, q_out)); CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToGravity(q_in, q_out)); CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToMass(q_in, m)); + CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= extwrench_estimator.JntToExtWrench(q_in,q_in2,ff_tau,wrench_out)); } void SolverTest::FkPosAndJacTest() { @@ -425,7 +538,7 @@ void SolverTest::IkSingularValueTest() q(0) = 0. ; q(1) = 0.5 ; q(2) = 0.4 ; - q(3) = -M_PI_2 ; + q(3) = -PI_2 ; q(4) = 0. ; q(5) = 0. ; q(6) = 0. ; @@ -452,7 +565,7 @@ void SolverTest::IkSingularValueTest() q(0) = 0. ; q(1) = 0.2 ; q(2) = 0.4 ; - q(3) = -M_PI_2 ; + q(3) = -PI_2 ; q(4) = 0. ; q(5) = 0. ; q(6) = 0. ; @@ -475,7 +588,7 @@ void SolverTest::IkSingularValueTest() q(0) = 0. ; q(1) = 0.5 ; q(2) = 0.4 ; - q(3) = -M_PI_2 ; + q(3) = -PI_2 ; q(4) = 0. ; q(5) = 0. ; q(6) = 0. ; @@ -544,7 +657,7 @@ void SolverTest::IkVelSolverWDLSTest() q(0) = 0. ; q(1) = 0.5 ; q(2) = 0.4 ; - q(3) = -M_PI_2 ; + q(3) = -PI_2 ; q(4) = 0. ; q(5) = 0. ; q(6) = 0. ; @@ -704,7 +817,178 @@ void SolverTest::FkPosAndIkPosLocal(Chain& chain,ChainFkSolverPos& fksolverpos, void SolverTest::VereshchaginTest() { - + std::cout << "KDL Vereshchagin Hybrid Dynamics Tests" < xDotdot(ns + 1); + // This solver's function returns Cartesian accelerations of links in robot base coordinates + vereshchaginSolver.getTransformedLinkAcceleration(xDotdot); + CPPUNIT_ASSERT(Equal(beta_energy(0), xDotdot[ns].vel(0), eps)); + CPPUNIT_ASSERT(Equal(beta_energy(1), xDotdot[ns].vel(1), eps)); + CPPUNIT_ASSERT(Equal(beta_energy(2), xDotdot[ns].vel(2), eps)); + CPPUNIT_ASSERT(Equal(beta_energy(5), xDotdot[ns].rot(2), eps)); + + // Additional getters for the intermediate solver's outputs: Useful for state- simulation and estimation purposes + // Magnitude of the constraint forces acting on the end-effector: Lagrange Multiplier + Eigen::VectorXd nu(number_of_constraints); + vereshchaginSolver.getContraintForceMagnitude(nu); + CPPUNIT_ASSERT(Equal(nu(0), 669693.30355, eps)); + CPPUNIT_ASSERT(Equal(nu(1), 5930.60826, eps)); + CPPUNIT_ASSERT(Equal(nu(2), -639.5238, eps)); + CPPUNIT_ASSERT(Equal(nu(3), 0.000, eps)); // constraint was not active in the task specification + CPPUNIT_ASSERT(Equal(nu(4), 0.000, eps)); // constraint was not active in the task specification + CPPUNIT_ASSERT(Equal(nu(5), 573.90485, eps)); + + // Total torque acting on each joint + JntArray total_tau(nj); + vereshchaginSolver.getTotalTorque(total_tau); + CPPUNIT_ASSERT(Equal(total_tau(0), 2013.3541, eps)); + CPPUNIT_ASSERT(Equal(total_tau(1), -6073.4999, eps)); + CPPUNIT_ASSERT(Equal(total_tau(2), 2227.4487, eps)); + CPPUNIT_ASSERT(Equal(total_tau(3), 56.87456, eps)); + CPPUNIT_ASSERT(Equal(total_tau(4), -11.3789, eps)); + CPPUNIT_ASSERT(Equal(total_tau(5), -6.05957, eps)); + CPPUNIT_ASSERT(Equal(total_tau(6), 569.0776, eps)); + + // ######################################################################################## + // Vereshchagin solver test 2 + // ######################################################################################## Vector constrainXLinear(1.0, 0.0, 0.0); Vector constrainXAngular(0.0, 0.0, 0.0); Vector constrainYLinear(0.0, 0.0, 0.0); @@ -747,7 +1031,7 @@ void SolverTest::VereshchaginTest() //Definition of solver and initial configuration //-------------------------------------------------------------------------------------// int numberOfConstraints = 1; - ChainIdSolver_Vereshchagin constraintSolver(chaindyn, twist1, numberOfConstraints); + ChainHdSolver_Vereshchagin constraintSolver(chaindyn, twist1, numberOfConstraints); //These arrays of joint values contain actual and desired values //actual is generated by a solver and integrator @@ -758,20 +1042,22 @@ void SolverTest::VereshchaginTest() JntArray jointPoses[k]; JntArray jointRates[k]; JntArray jointAccelerations[k]; - JntArray jointTorques[k]; + JntArray jointFFTorques[k]; + JntArray jointConstraintTorques[k]; for (int i = 0; i < k; i++) { JntArray jointValues(chaindyn.getNrOfJoints()); jointPoses[i] = jointValues; jointRates[i] = jointValues; jointAccelerations[i] = jointValues; - jointTorques[i] = jointValues; + jointFFTorques[i] = jointValues; + jointConstraintTorques[i] = jointValues; } // Initial arm position configuration/constraint JntArray jointInitialPose(chaindyn.getNrOfJoints()); jointInitialPose(0) = 0.0; // initial joint0 pose - jointInitialPose(1) = M_PI/6.0; //initial joint1 pose, negative in clockwise + jointInitialPose(1) = PI/6.0; //initial joint1 pose, negative in clockwise //j0=0.0, j1=pi/6.0 correspond to x = 0.2, y = -0.7464 //j0=2*pi/3.0, j1=pi/4.0 correspond to x = 0.44992, y = 0.58636 @@ -789,21 +1075,21 @@ void SolverTest::VereshchaginTest() double taskTimeConstant = 0.1; double simulationTime = 1*taskTimeConstant; double timeDelta = 0.01; - int status; const std::string msg = "Assertion failed, check matrix and array sizes"; for (double t = 0.0; t <=simulationTime; t = t + timeDelta) { - CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, constraintSolver.CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0], alpha, betha, externalNetForce, jointTorques[0])); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, constraintSolver.CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0], alpha, betha, externalNetForce, jointFFTorques[0], jointConstraintTorques[0])); //Integration(robot joint values for rates and poses; actual) at the given "instanteneous" interval for joint position and velocity. jointRates[0](0) = jointRates[0](0) + jointAccelerations[0](0) * timeDelta; //Euler Forward jointPoses[0](0) = jointPoses[0](0) + (jointRates[0](0) - jointAccelerations[0](0) * timeDelta / 2.0) * timeDelta; //Trapezoidal rule jointRates[0](1) = jointRates[0](1) + jointAccelerations[0](1) * timeDelta; //Euler Forward jointPoses[0](1) = jointPoses[0](1) + (jointRates[0](1) - jointAccelerations[0](1) * timeDelta / 2.0) * timeDelta; + jointFFTorques[0] = jointConstraintTorques[0]; //printf("time, j0_pose, j1_pose, j0_rate, j1_rate, j0_acc, j1_acc, j0_constraintTau, j1_constraintTau \n"); - printf("%f %f %f %f %f %f %f %f %f\n", t, jointPoses[0](0), jointPoses[0](1), jointRates[0](0), jointRates[0](1), jointAccelerations[0](0), jointAccelerations[0](1), jointTorques[0](0), jointTorques[0](1)); + printf("%f %f %f %f %f %f %f %f %f\n", t, jointPoses[0](0), jointPoses[0](1), jointRates[0](0), jointRates[0](1), jointAccelerations[0](0), jointAccelerations[0](1), jointConstraintTorques[0](0), jointConstraintTorques[0](1)); } } @@ -847,3 +1133,697 @@ void SolverTest::FkVelVectTest() CPPUNIT_ASSERT(Equal(v_out[chain1.getNrOfSegments()-1],f_out,1e-5)); } + +void SolverTest::FdSolverDevelopmentTest() +{ + int ret; + double eps=1.e-3; + + std::cout<<"KDL FD Solver Development Test for Motoman SIA10"< end_eff_force; // variable necessary for the control + Eigen::Matrix end_eff_pos_error; // variable necessary for the control + Eigen::Matrix end_eff_vel_error; // variable necessary for the control + + // Arm root acceleration (robot's base mounted on an even surface) + Vector linearAcc(0.0, 0.0, -9.81); Vector angularAcc(0.0, 0.0, 0.0); + + // Initialize kinematics solvers + ChainFkSolverPos_recursive fksolverpos(kukaLWR); + ChainFkSolverVel_recursive fksolvervel(kukaLWR); + ChainJntToJacSolver jacobian_solver(kukaLWR); + + // RNE ID solver for control purposes + KDL::ChainIdSolver_RNE IdSolver(kukaLWR, linearAcc); + + // Vereshchagin Hybrid Dynamics solver for simulation purposes + int numberOfConstraints = 6; + Jacobian alpha(numberOfConstraints); // Constraint Unit forces at the end-effector + JntArray beta(numberOfConstraints); // Acceleration energy at the end-effector + KDL::SetToZero(alpha); // Set to zero to deactivate all constraints + KDL::SetToZero(beta); // Set to zero to deactivate all constraints + Twist vereshchagin_root_Acc(-linearAcc, angularAcc); // Note: Vereshchagin solver takes root acc. with opposite sign comparead to the above FD and RNE solvers + ChainHdSolver_Vereshchagin constraintSolver(kukaLWR, vereshchagin_root_Acc, numberOfConstraints); + + // External Wrench Estimator + double sample_frequency = 1000.0; // Hz + double estimation_gain = 45.0; + double filter_constant = 0.5; + ChainExternalWrenchEstimator extwrench_estimator(kukaLWR, linearAcc, sample_frequency, estimation_gain, filter_constant); + + // Prepare test cases + std::vector jnt_pos; + std::vector wrench_reference; + + // Initialize random generator + std::random_device rd; //Will be used to obtain a seed for the random number engine + std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd() + std::uniform_real_distribution<> dis_force(-15.0, 15.0); + std::uniform_real_distribution<> dis_moment(-0.9, 0.9); + std::uniform_real_distribution<> dis_jnt_vel(-0.5, 0.5); + + // Set first test case + q(0) = 1.0; + q(1) = 0.0; + q(2) = 0.0; + q(3) = 4.71; + q(4) = 0.0; + q(5) = 1.57; + q(6) = 5.48; + jnt_pos.push_back(q); + wrench_reference.push_back(Wrench(Vector(dis_force(gen), dis_force(gen), dis_force(gen)), Vector(0.0, 0.0, 0.0))); // Ground-truth external wrench acting on the end-effector expressed in local end-effector's frame + + // Set second test case + q(0) = 2.96; + q(1) = 1.02; + q(2) = 6.15; + q(3) = 1.61; + q(4) = 0.22; + q(5) = 0.17; + q(6) = 0.01; + jnt_pos.push_back(q); + wrench_reference.push_back(Wrench(Vector(0.0, 0.0, 0.0), Vector(dis_moment(gen), dis_moment(gen), 0.0))); // expressed in local end-effector's frame + + // Set third test case + q(0) = 1.12; + q(1) = 0.66; + q(2) = 6.15; + q(3) = 4.09; + q(4) = 1.64; + q(5) = 0.12; + q(6) = 0.01; + jnt_pos.push_back(q); + wrench_reference.push_back(Wrench(Vector(dis_force(gen), dis_force(gen), dis_force(gen)), Vector(dis_moment(gen), 0.0, dis_moment(gen)))); // expressed in local end-effector's frame + + // ########################################################################################## + // Control and simulation + // ########################################################################################## + + // Control gains for a simple PD controller + double k_p = 1500.0; // Proportional + double k_d = 300.0; // Derivative + + // Time required to complete the task + double simulationTime = 0.4; // in seconds + double timeDelta = 1.0 / sample_frequency; // unit of seconds + + // Iterate over test cases + for (unsigned int i = 0; i < jnt_pos.size(); i++) + { + // Re-set control and simulation variables + q = jnt_pos[i]; + qd(0) = dis_jnt_vel(gen); + qd(1) = dis_jnt_vel(gen); + qd(2) = dis_jnt_vel(gen); + qd(3) = dis_jnt_vel(gen); + qd(4) = dis_jnt_vel(gen); + qd(5) = dis_jnt_vel(gen); + qd(6) = dis_jnt_vel(gen); + + end_eff_force.setZero(); + end_eff_pos_error.setZero(); + end_eff_vel_error.setZero(); + f_ext_base = f_ext_zero; + + // Initialize the estimator + extwrench_estimator.updateInternalDataStructures(); + extwrench_estimator.setInitialMomentum(q, qd); // sets the offset for future estimation (momentum calculation) + + // Set the desired Cartesian state + fksolverpos.JntToCart(q, end_effector_pose); + desired_end_eff_pose.p(0) = end_effector_pose.p(0) + 0.02; + desired_end_eff_pose.p(1) = end_effector_pose.p(1) + 0.02; + desired_end_eff_pose.p(2) = end_effector_pose.p(2) + 0.02; + desired_end_eff_twist.p.v(0) = 0.0; + desired_end_eff_twist.p.v(1) = 0.0; + desired_end_eff_twist.p.v(2) = 0.0; + + for (double t = 0.0; t <= simulationTime; t = t + timeDelta) + { + ret = jacobian_solver.JntToJac(q, jacobian_end_eff); + if (ret < 0) + { + std::cout << "Jacobian solver ERROR: " << ret << std::endl; + break; + } + + ret = fksolverpos.JntToCart(q, end_effector_pose); + if (ret < 0) + { + std::cout << "FK pos solver ERROR: " << ret << std::endl; + break; + } + + jnt_position_velocity.q = q; + jnt_position_velocity.qdot = qd; + ret = fksolvervel.JntToCart(jnt_position_velocity, end_eff_twist); + if (ret < 0) + { + std::cout << "FK vel solver ERROR: " << ret << std::endl; + break; + } + + end_eff_pos_error(0) = end_effector_pose.p(0) - desired_end_eff_pose.p(0); + end_eff_pos_error(1) = end_effector_pose.p(1) - desired_end_eff_pose.p(1); + end_eff_pos_error(2) = end_effector_pose.p(2) - desired_end_eff_pose.p(2); + + end_eff_vel_error(0) = end_eff_twist.p.v(0) - desired_end_eff_twist.p.v(0); + end_eff_vel_error(1) = end_eff_twist.p.v(1) - desired_end_eff_twist.p.v(1); + end_eff_vel_error(2) = end_eff_twist.p.v(2) - desired_end_eff_twist.p.v(2); + + end_eff_force = -end_eff_pos_error * k_p - end_eff_vel_error * k_d; + + // Compute gravity joint torques (hide external wrench from this dynamics calculation) + ret = IdSolver.CartToJnt(q, jnt_array_zero, jnt_array_zero, f_ext_zero, gravity_torque); + if (ret < 0) + { + std::cout << "KDL RNE solver ERROR: " << ret << std::endl; + break; + } + + // Compute joint control commands + command_torque.data = jacobian_end_eff.data.transpose() * end_eff_force; + command_torque.data += gravity_torque.data; + + // Start simulating the external force + if (t > 0.2) f_ext_base[ns - 1] = end_effector_pose.M * wrench_reference[i]; + + // Compute resultant joint accelerations that simulate robot's behaviour, given the command torques (add external wrench in this dynamics calculation) + ret = constraintSolver.CartToJnt(q, qd, qdd, alpha, beta, f_ext_base, command_torque, constraint_tau); + if (ret < 0) + { + std::cout << "KDL Vereshchagin solver ERROR: " << ret << std::endl; + break; + } + + // State integration: integrate from model accelerations to next joint state (positions and velocities) + qd.data = qd.data + qdd.data * timeDelta; // Euler Forward + q.data = q.data + qd.data * timeDelta; // Symplectic Euler + + // Saturate integrated joint position for full circle crossing + for (unsigned int j = 0; j < nj; j++) + { + q(j) = std::fmod(q(j), 360 * deg2rad); + if (q(j) < 0.0) q(j) += 360 * deg2rad; + } + + // Estimate external wrench + extwrench_estimator.JntToExtWrench(q, qd, command_torque, f_tool_estimated); + } + + // Inverse Force Kinematics + Eigen::Matrix wrench; + wrench(0) = f_ext_base[ns - 1](0); + wrench(1) = f_ext_base[ns - 1](1); + wrench(2) = f_ext_base[ns - 1](2); + wrench(3) = f_ext_base[ns - 1](3); + wrench(4) = f_ext_base[ns - 1](4); + wrench(5) = f_ext_base[ns - 1](5); + ext_torque_reference.data = jacobian_end_eff.data.transpose() * wrench; + + // Get estimated joint torque + extwrench_estimator.getEstimatedJntTorque(ext_torque_estimated); + + // ################################################################################## + // Final comparison + // ################################################################################## + CPPUNIT_ASSERT(Equal(f_tool_estimated(0), wrench_reference[i](0), eps_wrench)); + CPPUNIT_ASSERT(Equal(f_tool_estimated(1), wrench_reference[i](1), eps_wrench)); + CPPUNIT_ASSERT(Equal(f_tool_estimated(2), wrench_reference[i](2), eps_wrench)); + CPPUNIT_ASSERT(Equal(f_tool_estimated(3), wrench_reference[i](3), eps_wrench)); + CPPUNIT_ASSERT(Equal(f_tool_estimated(4), wrench_reference[i](4), eps_wrench)); + CPPUNIT_ASSERT(Equal(f_tool_estimated(5), wrench_reference[i](5), eps_wrench)); + + CPPUNIT_ASSERT(Equal(ext_torque_estimated(0), ext_torque_reference(0), eps_torque)); + CPPUNIT_ASSERT(Equal(ext_torque_estimated(1), ext_torque_reference(1), eps_torque)); + CPPUNIT_ASSERT(Equal(ext_torque_estimated(2), ext_torque_reference(2), eps_torque)); + CPPUNIT_ASSERT(Equal(ext_torque_estimated(3), ext_torque_reference(3), eps_torque)); + CPPUNIT_ASSERT(Equal(ext_torque_estimated(4), ext_torque_reference(4), eps_torque)); + CPPUNIT_ASSERT(Equal(ext_torque_estimated(5), ext_torque_reference(5), eps_torque)); + CPPUNIT_ASSERT(Equal(ext_torque_estimated(6), ext_torque_reference(6), eps_torque)); + } + + return; +} diff --git a/orocos_kdl/tests/solvertest.hpp b/orocos_kdl/tests/solvertest.hpp index 26c310636..8da18319f 100644 --- a/orocos_kdl/tests/solvertest.hpp +++ b/orocos_kdl/tests/solvertest.hpp @@ -15,9 +15,13 @@ #include #include #include -#include +#include #include #include +#include +#include +#include +#include using namespace KDL; @@ -30,10 +34,15 @@ class SolverTest : public CppUnit::TestFixture CPPUNIT_TEST(FkVelAndIkVelTest ); CPPUNIT_TEST(FkPosAndIkPosTest ); CPPUNIT_TEST(VereshchaginTest ); + CPPUNIT_TEST(ExternalWrenchEstimatorTest ); CPPUNIT_TEST(IkSingularValueTest ); CPPUNIT_TEST(IkVelSolverWDLSTest ); CPPUNIT_TEST(FkPosVectTest ); CPPUNIT_TEST(FkVelVectTest ); + CPPUNIT_TEST(FdSolverDevelopmentTest ); + CPPUNIT_TEST(FdSolverConsistencyTest ); + CPPUNIT_TEST(LDLdecompTest); + CPPUNIT_TEST(FdAndVereshchaginSolversConsistencyTest ); CPPUNIT_TEST(UpdateChainTest ); CPPUNIT_TEST_SUITE_END(); @@ -46,15 +55,20 @@ class SolverTest : public CppUnit::TestFixture void FkVelAndIkVelTest(); void FkPosAndIkPosTest(); void VereshchaginTest(); + void ExternalWrenchEstimatorTest(); void IkSingularValueTest() ; void IkVelSolverWDLSTest(); void FkPosVectTest(); void FkVelVectTest(); + void FdSolverDevelopmentTest(); + void FdSolverConsistencyTest(); + void LDLdecompTest(); + void FdAndVereshchaginSolversConsistencyTest(); void UpdateChainTest(); private: - Chain chain1,chain2,chain3,chain4, chaindyn,motomansia10; + Chain chain1, chain2, chain3, chain4, chaindyn, motomansia10, motomansia10dyn, kukaLWR; void FkPosAndJacLocal(Chain& chain,ChainFkSolverPos& fksolverpos,ChainJntToJacSolver& jacsolver); void FkVelAndJacLocal(Chain& chain, ChainFkSolverVel& fksolvervel, ChainJntToJacSolver& jacsolver); diff --git a/orocos_kdl/tests/test-runner.cpp b/orocos_kdl/tests/test-runner.cpp index 988586787..db23244f2 100644 --- a/orocos_kdl/tests/test-runner.cpp +++ b/orocos_kdl/tests/test-runner.cpp @@ -1,21 +1,6 @@ -// Copyright (C) 2007 Klaas Gadeyne -// -// This program is free software; you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation; either version 2 of the License, or -// (at your option) any later version. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program; if not, write to the Free Software -// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. -// +// Copyright (C) 2007 Klaas Gadeyne -/* Modified from orocos cppunit test code, also published under GPLV2 +/* Modified from orocos cppunit test code begin : Mon January 10 2005 copyright : (C) 2005 Peter Soetens email : peter.soetens@mech.kuleuven.ac.be diff --git a/orocos_kdl/tests/toolkittest.cpp b/orocos_kdl/tests/toolkittest.cpp index eb1e47965..43e36976c 100644 --- a/orocos_kdl/tests/toolkittest.cpp +++ b/orocos_kdl/tests/toolkittest.cpp @@ -11,7 +11,6 @@ using namespace RTT; using namespace KDL; -using namespace std; int ORO_main(int argc, char** argv) { @@ -99,5 +98,3 @@ int ORO_main(int argc, char** argv) return retval; } - - diff --git a/orocos_kdl/tests/treeinvdyntest.cpp b/orocos_kdl/tests/treeinvdyntest.cpp new file mode 100644 index 000000000..c6f5b9cf8 --- /dev/null +++ b/orocos_kdl/tests/treeinvdyntest.cpp @@ -0,0 +1,177 @@ +#include "treeinvdyntest.hpp" +#include +#include +#include +#include +#include +#include + + +CPPUNIT_TEST_SUITE_REGISTRATION( TreeInvDynTest ); + +using namespace KDL; +using std::cout; +using std::endl; + +void TreeInvDynTest::setUp() +{ + srand( (unsigned)time( NULL )); + + //spatial inertia (just to test dynamics) + RigidBodyInertia inertia(0.3, Vector(0.0, 0.1, 0.0), RotationalInertia(0.005, 0.001, 0.001)); + + //create chain #1 + chain1.addSegment(Segment("Segment 11", Joint("Joint 11", Joint::RotZ), + Frame(Vector(0.0,0.0,0.0)), inertia)); + chain1.addSegment(Segment("Segment 12", Joint("Joint 12", Joint::RotX), + Frame(Vector(0.0,0.0,0.9)), inertia)); + chain1.addSegment(Segment("Segment 13", Joint("Joint 13", Joint::None), + Frame(Vector(-0.4,0.0,0.0)))); + chain1.addSegment(Segment("Segment 14", Joint("Joint 14", Joint::RotX), + Frame(Vector(0.0,0.0,1.2)), inertia)); + chain1.addSegment(Segment("Segment 15", Joint("Joint 15", Joint::None), + Frame(Vector(0.4,0.0,0.0)), inertia)); + chain1.addSegment(Segment("Segment 16", Joint("Joint 16", Joint::RotZ), + Frame(Vector(0.0,0.0,1.4)), inertia)); + chain1.addSegment(Segment("Segment 17", Joint("Joint 17", Joint::RotX), + Frame(Vector(0.0,0.0,0.0)), inertia)); + chain1.addSegment(Segment("Segment 18", Joint("Joint 18", Joint::RotZ), + Frame(Vector(0.0,0.0,0.4)), inertia)); + chain1.addSegment(Segment("Segment 19", Joint("Joint 19", Joint::None), + Frame(Vector(0.0,0.0,0.0)), inertia)); + + //create chain #2 + chain2.addSegment(Segment("Segment 21", Joint("Joint 21", Joint::RotZ), + Frame(Vector(0.0,0.0,0.5)), inertia)); + chain2.addSegment(Segment("Segment 22", Joint("Joint 22", Joint::RotX), + Frame(Vector(0.0,0.0,0.4)), inertia)); + chain2.addSegment(Segment("Segment 23", Joint("Joint 23", Joint::RotX), + Frame(Vector(0.0,0.0,0.3)), inertia)); + chain2.addSegment(Segment("Segment 24", Joint("Joint 24", Joint::RotX), + Frame(Vector(0.0,0.0,0.2)), inertia)); + chain2.addSegment(Segment("Segment 25", Joint("Joint 25", Joint::RotZ), + Frame(Vector(0.0,0.0,0.1)), inertia)); + + //create tree as two chains attached to the root + tree = Tree(); + tree.addChain(chain1, tree.getRootSegment()->first); + tree.addChain(chain2, tree.getRootSegment()->first); + + //create a simple "y-shaped" tree + m=1.0, Iz=0.05, L=0.5; //geometric and dynamic parameters + Segment s1("S1", Joint("q1", Joint::RotZ), Frame(), RigidBodyInertia(m, Vector(L,0,0), RotationalInertia(0,0,Iz))); + Segment s2("S2", Joint("q2", Vector(L,0,0), Vector(0,0,1), Joint::RotAxis), Frame(Vector(L,0,0)), RigidBodyInertia(m, Vector(L/2,0,0), RotationalInertia(0,0,Iz))); + Segment s3("S3", Joint("q3", Vector(2*L,0,0), Vector(0,0,1), Joint::RotAxis), Frame(Vector(2*L,0,0)), RigidBodyInertia(m, Vector(L/2,0,0), RotationalInertia(0,0,Iz))); + ytree = Tree("root"); + ytree.addSegment(s1, "root"); + ytree.addSegment(s2, "S1"); + ytree.addSegment(s3, "S1"); +} + + +void TreeInvDynTest::tearDown() { } + + +void TreeInvDynTest::UpdateTreeTest() { + // std::cout << "Tree: " << endl << tree2str(tree) << endl; //NOTE: tree2str is not available as I implemented it in another "parallel" commit + std::cout << "\nTree: " << endl << tree << endl; + + Tree temp_tree(tree); + TreeIdSolver_RNE solver(temp_tree, Vector::Zero()); + JntArray q, qd, qdd, tau; + + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau)); + q.resize(temp_tree.getNrOfJoints()); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau)); + qd.resize(temp_tree.getNrOfJoints()); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau)); + qdd.resize(temp_tree.getNrOfJoints()); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau)); + tau.resize(temp_tree.getNrOfJoints()); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau)); + + temp_tree.addSegment(Segment("extra"), temp_tree.getRootSegment()->first); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau)); + solver.updateInternalDataStructures(); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau)); +} + + +void TreeInvDynTest::TwoChainsTest() { + Vector gravity(0,0,-9.8); + TreeIdSolver_RNE solver(tree, gravity); + ChainIdSolver_RNE solver1(chain1, gravity); + ChainIdSolver_RNE solver2(chain2, gravity); + + unsigned int nt = tree.getNrOfJoints(); + unsigned int n1 = chain1.getNrOfJoints(); + unsigned int n2 = chain2.getNrOfJoints(); + + //Check that sizes are consistent -- otherwise following code does not make sense! + CPPUNIT_ASSERT_EQUAL(nt, n1+n2); + + JntArray q(nt), qd(nt), qdd(nt), tau(nt); //data related to tree + JntArray q1(n1), qd1(n1), qdd1(n1), tau1(n1); //data related to chain1 + JntArray q2(n2), qd2(n2), qdd2(n2), tau2(n2); //data related to chain2 + + unsigned int iterations = 100; + while(iterations-- > 0) { + //Randomize joint vectors + for(unsigned int i=0; i 0) { + //Randomize joint vectors + for(unsigned int i=0; i + +#include +#include + + +using namespace KDL; + +class TreeInvDynTest : public CppUnit::TestFixture +{ + CPPUNIT_TEST_SUITE(TreeInvDynTest); + CPPUNIT_TEST(UpdateTreeTest); + CPPUNIT_TEST(TwoChainsTest); + CPPUNIT_TEST(YTreeTest); + CPPUNIT_TEST_SUITE_END(); + +public: + void setUp(); + void tearDown(); + + void UpdateTreeTest(); + void TwoChainsTest(); + void YTreeTest(); + +private: + Chain chain1,chain2; + Tree tree, ytree; + double m, Iz, L; +}; +#endif diff --git a/orocos_kdl/tests/zxxzxztest.cpp b/orocos_kdl/tests/zxxzxztest.cpp index 9e2c4299a..ea32f643e 100644 --- a/orocos_kdl/tests/zxxzxztest.cpp +++ b/orocos_kdl/tests/zxxzxztest.cpp @@ -6,8 +6,14 @@ #include #include +#include + using namespace KDL; -using namespace std; + +using std::cerr; +using std::cout; +using std::endl; + /** * testing of forward and inverse position kinematics of the routines. * Also tests everything for all configurations. diff --git a/orocos_kinematics_dynamics/CMakeLists.txt b/orocos_kinematics_dynamics/CMakeLists.txt deleted file mode 100644 index 2b2e95004..000000000 --- a/orocos_kinematics_dynamics/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(orocos_kinematics_dynamics) -find_package(catkin REQUIRED) -catkin_metapackage() diff --git a/orocos_kinematics_dynamics/package.xml b/orocos_kinematics_dynamics/package.xml deleted file mode 100644 index 1a80ce5b3..000000000 --- a/orocos_kinematics_dynamics/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - orocos_kinematics_dynamics - 1.4.0 - - This package depends on a recent version of the Kinematics and Dynamics - Library (KDL), distributed by the Orocos Project. It is a meta-package that - depends on kdl which contains the c++ version and pykdl which contains the - generated python bindings. - - Orocos Developers - LGPL - - catkin - - orocos_kdl - python_orocos_kdl - - - diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index f60f20102..64fdd2159 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -1,30 +1,67 @@ -cmake_minimum_required(VERSION 2.4.6) +cmake_minimum_required(VERSION 3.12.0) +if(POLICY CMP0048) + cmake_policy(SET CMP0048 NEW) +endif() +# Avoid warning from pybind11 on Windows +if(POLICY CMP0054) + cmake_policy(SET CMP0054 NEW) +endif() +if(POLICY CMP0057) + cmake_policy(SET CMP0057 NEW) # support IN_LISTS which is required for PyBind11 2.6 and newer +endif() -project(python_orocos_kdl) +project(python_orocos_kdl VERSION 1.5.1) -find_package(orocos_kdl) +# find a matching version of orocos_kdl +find_package(orocos_kdl ${PROJECT_VERSION} EXACT REQUIRED) include_directories(${orocos_kdl_INCLUDE_DIRS}) link_directories(${orocos_kdl_LIBRARY_DIRS}) -SET(PYTHON_VERSION 2 CACHE STRING "Python Version") -find_package(PythonInterp ${PYTHON_VERSION} REQUIRED) -find_package(PythonLibs ${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR} REQUIRED) -execute_process(COMMAND ${PYTHON_EXECUTABLE} -c "from distutils.sysconfig import get_python_lib; print(get_python_lib(plat_specific=True, prefix=''))" OUTPUT_VARIABLE PYTHON_SITE_PACKAGES OUTPUT_STRIP_TRAILING_WHITESPACE) -list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) -find_package(SIP REQUIRED) -include(SIPMacros) - -include_directories(${SIP_INCLUDE_DIR} ${PYTHON_INCLUDE_DIRS}) +if(DEFINED ENV{ROS_PYTHON_VERSION}) + SET(PYTHON_VERSION $ENV{ROS_PYTHON_VERSION} CACHE STRING "Python Version") +else() + SET(PYTHON_VERSION 3 CACHE STRING "Python Version") +endif() -file(GLOB SIP_FILES "${CMAKE_CURRENT_SOURCE_DIR}/*.sip") -set(SIP_INCLUDES ${SIP_FILES}) -set(SIP_EXTRA_OPTIONS "-o") -set(PYTHON_SITE_PACKAGES_INSTALL_DIR ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITE_PACKAGES}) -add_sip_python_module(PyKDL PyKDL/PyKDL.sip ${orocos_kdl_LIBRARIES}) +set(PYBIND11_PYTHON_VERSION ${PYTHON_VERSION} CACHE STRING "Python version used by PyBind11") -install(FILES package.xml DESTINATION share/python_orocos_kdl) +find_package(Python ${PYTHON_VERSION} COMPONENTS Interpreter Development REQUIRED) +# get_python_lib in python3 produces path which isn't in sys.path: https://bugs.launchpad.net/ubuntu/+source/python3-stdlib-extensions/+bug/1832215 +# execute_process(COMMAND ${PYTHON_EXECUTABLE} -c "from distutils.sysconfig import get_python_lib; print(get_python_lib(plat_specific=True, prefix=''))" OUTPUT_VARIABLE PYTHON_SITE_PACKAGES OUTPUT_STRIP_TRAILING_WHITESPACE) +set(PYTHON_SITE_PACKAGES_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/lib/python${Python_VERSION_MAJOR}.${Python_VERSION_MINOR}/dist-packages") # This might be overridden below if built with catkin. +set(LIBRARY_NAME "PyKDL") +# catkin-specific configuration (optional) find_package(catkin QUIET) if(catkin_FOUND) - catkin_add_env_hooks(python_orocos_kdl_site_packages SHELLS bash tcsh zsh DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/env-hooks) + catkin_package( + SKIP_CMAKE_CONFIG_GENERATION + SKIP_PKG_CONFIG_GENERATION + ) + set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${PYTHON_INSTALL_DIR}) + set(PYTHON_SITE_PACKAGES_INSTALL_DIR "${PYTHON_INSTALL_DIR}") +endif() + +# Build the module +if(WIN32) + set(PYTHON_MODULE_EXTENSION ".pyd") +else() + set(PYTHON_MODULE_EXTENSION ".so") +endif() + +find_package(pybind11 2.6 QUIET) +if(NOT ${pybind11_FOUND}) + message(STATUS "pybind11 not found, building from source") + add_subdirectory(pybind11) +else() + message(STATUS "pybind11 found, building against installed version") endif() +pybind11_add_module(${LIBRARY_NAME} + PyKDL/PyKDL.h + PyKDL/PyKDL.cpp + PyKDL/frames.cpp + PyKDL/kinfam.cpp + PyKDL/framevel.cpp + PyKDL/dynamics.cpp) +target_link_libraries(${LIBRARY_NAME} PRIVATE ${orocos_kdl_LIBRARIES}) +install(TARGETS ${LIBRARY_NAME} DESTINATION "${PYTHON_SITE_PACKAGES_INSTALL_DIR}") diff --git a/python_orocos_kdl/INSTALL.md b/python_orocos_kdl/INSTALL.md new file mode 100644 index 000000000..becd791b0 --- /dev/null +++ b/python_orocos_kdl/INSTALL.md @@ -0,0 +1,41 @@ +# Install instructions + +These install instructions are focused on Debian/Ubuntu systems. + +## Shared instructions + +1. Follow the shared instructions of the C++ library from [orocos_kdl/INSTALL.md](../orocos_kdl/INSTALL.md#shared-instructions) +2. Install the `future` and `psutil` module: `sudo apt-get install python3-psutil python3-future` +3. (Optional) Install `Sphinx` to generate API-documentation: `sudo apt-get install python3-sphinx` + +## Compilation + +### With catkin + +1. Clone the repository inside the workspace +2. Initialize the [PyBind11](https://github.com/pybind/pybind11) submodule: `git submodule update --init` +3. Build with your catkin tool of preference. This will also build the C++ library +4. Source the workspace +5. (Optional) To generate the API-documentation use either [rosdoc_lite](http://wiki.ros.org/rosdoc_lite) or +[catkin_tools_document](https://github.com/mikepurvis/catkin_tools_document) + +### Without catkin + +1. Clone the repository where you want +2. Initialize the [PyBind11](https://github.com/pybind/pybind11) submodule: `git submodule update --init` +3. Follow the mandatory instruction to compile the C++ library from [orocos_kdl/INSTALL.md](../orocos_kdl/INSTALL.md#without-catkin) +4. Create a new build folder (it is always better not to build in the source folder): `mkdir build` +5. Go to the build folder `cd build` +6. Execute cmake: `cmake ..` + - (Optional) Adapt `CMAKE_INSTALL_PREFIX` to the desired installation directory + - (Optional) To change the build type, add: `-DCMAKE_BUILD_TYPE=` +7. Compile: `make` +8. Install the python bindings: `sudo make install` +9. Make sure `LD_LIBRARY_PATH` is set correctly: `export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib`. Add this also +to your `.bashrc`. +10. Execute `ldconfig`: `sudo ldconfig` +11. (Optional) Execute tests: `python3 ../tests/PyKDLtest.py` +12. (Optional) To create the API-documentation: `sphinx-build ../doc docs`. The API-documentation will be generated at +`/docs`. + +To uninstall the python bindings: `sudo make uninstall` diff --git a/python_orocos_kdl/PyKDL/PyKDL.cpp b/python_orocos_kdl/PyKDL/PyKDL.cpp new file mode 100644 index 000000000..eb286272a --- /dev/null +++ b/python_orocos_kdl/PyKDL/PyKDL.cpp @@ -0,0 +1,40 @@ +//Copyright (C) 2007 Ruben Smits +// +//Version: 1.0 +//Author: Ruben Smits Ruben Smits +//Author: Zihan Chen +//Author: Matthijs van der Burgh +//Maintainer: Ruben Smits Ruben Smits +//Maintainer: Matthijs van der Burgh +//URL: http://www.orocos.org/kdl +// +//This library is free software; you can redistribute it and/or +//modify it under the terms of the GNU Lesser General Public +//License as published by the Free Software Foundation; either +//version 2.1 of the License, or (at your option) any later version. +// +//This library is distributed in the hope that it will be useful, +//but WITHOUT ANY WARRANTY; without even the implied warranty of +//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +//Lesser General Public License for more details. +// +//You should have received a copy of the GNU Lesser General Public +//License along with this library; if not, write to the Free Software +//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +#include +#include "PyKDL.h" + +namespace py = pybind11; + + +PYBIND11_MODULE(PyKDL, m) +{ + m.doc() = "Orocos KDL Python wrapper"; // optional module docstring + m.attr("__version__") = KDL_VERSION_STRING; + init_frames(m); + init_framevel(m); + init_kinfam(m); + init_dynamics(m); +} diff --git a/python_orocos_kdl/PyKDL/PyKDL.sip b/python_orocos_kdl/PyKDL/PyKDL.h similarity index 52% rename from python_orocos_kdl/PyKDL/PyKDL.sip rename to python_orocos_kdl/PyKDL/PyKDL.h index 1de04528c..b72a81bd0 100644 --- a/python_orocos_kdl/PyKDL/PyKDL.sip +++ b/python_orocos_kdl/PyKDL/PyKDL.h @@ -1,8 +1,11 @@ -//Copyright (C) 2007 Ruben Smits +//Copyright (C) 2007 Ruben Smits // //Version: 1.0 -//Author: Ruben Smits -//Maintainer: Ruben Smits +//Author: Ruben Smits Ruben Smits +//Author: Zihan Chen +//Author: Matthijs van der Burgh +//Maintainer: Ruben Smits Ruben Smits +//Maintainer: Matthijs van der Burgh //URL: http://www.orocos.org/kdl // //This library is free software; you can redistribute it and/or @@ -20,13 +23,13 @@ //Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -%Module(name = PyKDL,version=0,keyword_arguments="All") +#include +#include +#include +#include -%License(type="LGPL",licensee="Ruben Smits",signature="ruben@intermodalics.eu",timestamp="2014") -%Include std_string.sip -%Include frames.sip -%Include kinfam.sip -%Include framevel.sip - -%Include dynamics.sip +void init_frames(pybind11::module &m); +void init_framevel(pybind11::module &m); +void init_kinfam(pybind11::module &m); +void init_dynamics(pybind11::module &m); diff --git a/python_orocos_kdl/PyKDL/dynamics.cpp b/python_orocos_kdl/PyKDL/dynamics.cpp new file mode 100644 index 000000000..46d0f503a --- /dev/null +++ b/python_orocos_kdl/PyKDL/dynamics.cpp @@ -0,0 +1,93 @@ +//Copyright (C) 2007 Ruben Smits +// +//Version: 1.0 +//Author: Ruben Smits Ruben Smits +//Author: Zihan Chen +//Author: Matthijs van der Burgh +//Maintainer: Ruben Smits Ruben Smits +//Maintainer: Matthijs van der Burgh +//URL: http://www.orocos.org/kdl +// +//This library is free software; you can redistribute it and/or +//modify it under the terms of the GNU Lesser General Public +//License as published by the Free Software Foundation; either +//version 2.1 of the License, or (at your option) any later version. +// +//This library is distributed in the hope that it will be useful, +//but WITHOUT ANY WARRANTY; without even the implied warranty of +//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +//Lesser General Public License for more details. +// +//You should have received a copy of the GNU Lesser General Public +//License along with this library; if not, write to the Free Software +//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +#include +#include +#include +#include +#include +#include "PyKDL.h" + +namespace py = pybind11; +using namespace KDL; + + +void init_dynamics(pybind11::module &m) +{ + // -------------------- + // JntSpaceInertiaMatrix + // -------------------- + py::class_ jnt_space_inertia_matrix(m, "JntSpaceInertiaMatrix"); + jnt_space_inertia_matrix.def(py::init<>()); + jnt_space_inertia_matrix.def(py::init()); + jnt_space_inertia_matrix.def(py::init()); + jnt_space_inertia_matrix.def("resize", &JntSpaceInertiaMatrix::resize, py::arg("new_size")); + jnt_space_inertia_matrix.def("rows", &JntSpaceInertiaMatrix::rows); + jnt_space_inertia_matrix.def("columns", &JntSpaceInertiaMatrix::columns); + jnt_space_inertia_matrix.def("__getitem__", [](const JntSpaceInertiaMatrix &jm, std::tuple idx) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || (unsigned int)i >= jm.rows() || j < 0 || (unsigned int)j >= jm.columns()) + throw py::index_error("Inertia index out of range"); + + return jm((unsigned int)i, (unsigned int)j); + }); + jnt_space_inertia_matrix.def("__setitem__", [](JntSpaceInertiaMatrix &jm, std::tuple idx, double value) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || (unsigned int)i >= jm.rows() || j < 0 || (unsigned int)j >= jm.columns()) + throw py::index_error("Inertia index out of range"); + + jm((unsigned int)i, (unsigned int)j) = value; + }); + jnt_space_inertia_matrix.def("__repr__", [](const JntSpaceInertiaMatrix &jm) + { + std::ostringstream oss; + oss << jm; + return oss.str(); + }); + jnt_space_inertia_matrix.def(py::self == py::self); + + m.def("Add", (void (*)(const JntSpaceInertiaMatrix&, const JntSpaceInertiaMatrix&, JntSpaceInertiaMatrix&)) &KDL::Add, py::arg("src1"), py::arg("src2"), py::arg("dest")); + m.def("Subtract", (void (*)(const JntSpaceInertiaMatrix&, const JntSpaceInertiaMatrix&,JntSpaceInertiaMatrix&)) &KDL::Subtract, py::arg("src1"), py::arg("src2"), py::arg("dest")); + m.def("Multiply", (void (*)(const JntSpaceInertiaMatrix&, const double&, JntSpaceInertiaMatrix&)) &KDL::Multiply, py::arg("src"), py::arg("factor"), py::arg("dest")); + m.def("Divide", (void (*)(const JntSpaceInertiaMatrix&, const double&, JntSpaceInertiaMatrix&)) &KDL::Divide, py::arg("src"), py::arg("factor"), py::arg("dest")); + m.def("Multiply", (void (*)(const JntSpaceInertiaMatrix&, const JntArray&, JntArray&)) &KDL::Multiply, py::arg("src"), py::arg("vec"), py::arg("dest")); + m.def("SetToZero", (void (*)(JntSpaceInertiaMatrix&)) &KDL::SetToZero, py::arg("matrix")); + m.def("Equal", (bool (*)(const JntSpaceInertiaMatrix&, const JntSpaceInertiaMatrix&, double)) &KDL::Equal, + py::arg("src1"), py::arg("src2"), py::arg("eps")=epsilon); + + + // -------------------- + // ChainDynParam + // -------------------- + py::class_ chain_dyn_param(m, "ChainDynParam"); + chain_dyn_param.def(py::init()); + chain_dyn_param.def("JntToCoriolis", &ChainDynParam::JntToCoriolis, py::arg("q"), py::arg("q_dot"), py::arg("coriolis")); + chain_dyn_param.def("JntToMass", &ChainDynParam::JntToMass, py::arg("q"), py::arg("H")); + chain_dyn_param.def("JntToGravity", &ChainDynParam::JntToGravity, py::arg("q"), py::arg("gravity")); +} diff --git a/python_orocos_kdl/PyKDL/dynamics.sip b/python_orocos_kdl/PyKDL/dynamics.sip deleted file mode 100644 index e0096ddc8..000000000 --- a/python_orocos_kdl/PyKDL/dynamics.sip +++ /dev/null @@ -1,75 +0,0 @@ -//Copyright (C) 2007 Ruben Smits -// -//Version: 1.0 -//Author: Ruben Smits -//Maintainer: Ruben Smits -//URL: http://www.orocos.org/kdl -// -//This library is free software; you can redistribute it and/or -//modify it under the terms of the GNU Lesser General Public -//License as published by the Free Software Foundation; either -//version 2.1 of the License, or (at your option) any later version. -// -//This library is distributed in the hope that it will be useful, -//but WITHOUT ANY WARRANTY; without even the implied warranty of -//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -//Lesser General Public License for more details. -// -//You should have received a copy of the GNU Lesser General Public -//License along with this library; if not, write to the Free Software -//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA - -%Include std_string.sip - -class JntSpaceInertiaMatrix { -%TypeHeaderCode -#include -using namespace KDL; -%End -public: - JntSpaceInertiaMatrix(); - JntSpaceInertiaMatrix(int size); - JntSpaceInertiaMatrix(const JntSpaceInertiaMatrix& arg); - void resize(unsigned int newSize); - unsigned int rows()const; - unsigned int columns()const; - //JntSpaceInertiaMatrix& operator = ( const JntSpaceInertiaMatrix& arg); - double __getitem__(SIP_PYTUPLE); -%MethodCode - int i,j; - PyArg_ParseTuple(a0,"ii",&i,&j); - if (i < 0 || j < 0 || i > (int)sipCpp->rows() || j >= (int)sipCpp->columns()) { - PyErr_SetString(PyExc_IndexError, "Inertia index out of range"); - return NULL; - } - sipRes=(*sipCpp)(i,j); -%End - //double operator()(unsigned int i,unsigned int j)const; - //double& operator()(unsigned int i,unsigned int j); - //bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2); - //bool operator!=(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2); - -}; -void Add(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest); -void Subtract(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest); -void Multiply(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest); -void Divide(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest); -void Multiply(const JntSpaceInertiaMatrix& src, const JntArray& vec, JntArray& dest); -void SetToZero(JntSpaceInertiaMatrix& matrix); -bool Equal(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,double eps=epsilon); -bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2); - -class ChainDynParam { - -%TypeHeaderCode -#include -using namespace KDL; -%End - -public: - ChainDynParam(const Chain& chain, Vector _grav); - - int JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis); - int JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H); - int JntToGravity(const JntArray &q,JntArray &gravity); -}; diff --git a/python_orocos_kdl/PyKDL/frames.cpp b/python_orocos_kdl/PyKDL/frames.cpp new file mode 100644 index 000000000..9cbee1b97 --- /dev/null +++ b/python_orocos_kdl/PyKDL/frames.cpp @@ -0,0 +1,501 @@ +//Copyright (C) 2007 Ruben Smits +// +//Version: 1.0 +//Author: Ruben Smits Ruben Smits +//Author: Zihan Chen +//Author: Matthijs van der Burgh +//Maintainer: Ruben Smits Ruben Smits +//Maintainer: Matthijs van der Burgh +//URL: http://www.orocos.org/kdl +// +//This library is free software; you can redistribute it and/or +//modify it under the terms of the GNU Lesser General Public +//License as published by the Free Software Foundation; either +//version 2.1 of the License, or (at your option) any later version. +// +//This library is distributed in the hope that it will be useful, +//but WITHOUT ANY WARRANTY; without even the implied warranty of +//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +//Lesser General Public License for more details. +// +//You should have received a copy of the GNU Lesser General Public +//License along with this library; if not, write to the Free Software +//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +#include +#include +#include "PyKDL.h" + +namespace py = pybind11; +using namespace KDL; + + +void init_frames(py::module &m) +{ + // -------------------- + // Vector + // -------------------- + py::class_ vector(m, "Vector"); + vector.def(py::init<>()); + vector.def(py::init(), py::arg("x"), py::arg("y"), py::arg("z")); + vector.def(py::init()); + vector.def("x", (void (Vector::*)(double)) &Vector::x, py::arg("value")); + vector.def("y", (void (Vector::*)(double)) &Vector::y, py::arg("value")); + vector.def("z", (void (Vector::*)(double)) &Vector::z, py::arg("value")); + vector.def("x", (double (Vector::*)(void) const) &Vector::x); + vector.def("y", (double (Vector::*)(void) const) &Vector::y); + vector.def("z", (double (Vector::*)(void) const) &Vector::z); + vector.def("__getitem__", [](const Vector &v, int i) + { + if (i < 0 || i > 2) + throw py::index_error("Vector index out of range"); + + return v(i); + }, py::arg("index")); + vector.def("__setitem__", [](Vector &v, int i, double value) + { + if (i < 0 || i > 2) + throw py::index_error("Vector index out of range"); + + v(i) = value; + }, py::arg("index"), py::arg("value")); + vector.def("__repr__", [](const Vector &v) + { + std::ostringstream oss; + oss << v; + return oss.str(); + }); + vector.def("ReverseSign", &Vector::ReverseSign); + vector.def(py::self -= py::self); + vector.def(py::self += py::self); + vector.def(py::self + py::self); + vector.def(py::self - py::self); + vector.def(py::self * double()); + vector.def(double() * py::self); + vector.def(py::self / double()); + vector.def(py::self * py::self); + vector.def(py::self == py::self); + vector.def(py::self != py::self); + vector.def(py::hash(py::self)); + vector.def("__neg__", [](const Vector &a) + { + return operator-(a); + }, py::is_operator()); + vector.def("__copy__", [](const Vector& self) + { + return Vector(self); + }); + vector.def("__deepcopy__", [](const Vector& self, py::dict) + { + return Vector(self); + }, py::arg("memo")); + vector.def_static("Zero", &Vector::Zero); + vector.def("Norm", &Vector::Norm, py::arg("eps")=epsilon); + vector.def("Normalize", &Vector::Normalize, py::arg("eps")=epsilon); + vector.def(py::pickle( + [](const Vector &v) + { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(v.x(), v.y(), v.z()); + }, + [](py::tuple t) + { // __setstate__ + if (t.size() != 3) + throw std::runtime_error("Invalid state!"); + + /* Create a new C++ instance */ + Vector v(t[0].cast(), t[1].cast(), t[2].cast()); + return v; + })); + + m.def("SetToZero", (void (*)(Vector&)) &KDL::SetToZero, py::arg("vector")); + m.def("dot", (double (*)(const Vector&, const Vector&)) &KDL::dot); + m.def("Equal", (bool (*)(const Vector&, const Vector&, double)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + + + // -------------------- + // Wrench + // -------------------- + py::class_ wrench(m, "Wrench"); + wrench.def(py::init<>()); + wrench.def(py::init(), py::arg("force"), py::arg("torque")); + wrench.def(py::init()); + wrench.def_readwrite("force", &Wrench::force); + wrench.def_readwrite("torque", &Wrench::torque); + wrench.def("__getitem__", [](const Wrench &t, int i) + { + if (i < 0 || i > 5) + throw py::index_error("Wrench index out of range"); + + return t(i); + }, py::arg("index")); + wrench.def("__setitem__", [](Wrench &t, int i, double value) + { + if (i < 0 || i > 5) + throw py::index_error("Wrench index out of range"); + + t(i) = value; + }, py::arg("index"), py::arg("value")); + wrench.def("__repr__", [](const Wrench &t) + { + std::ostringstream oss; + oss << t; + return oss.str(); + }); + wrench.def("__copy__", [](const Wrench& self) + { + return Wrench(self); + }); + wrench.def("__deepcopy__", [](const Wrench& self, py::dict) + { + return Wrench(self); + }, py::arg("memo")); + wrench.def_static("Zero", &Wrench::Zero); + wrench.def("ReverseSign", &Wrench::ReverseSign); + wrench.def("RefPoint", &Wrench::RefPoint, py::arg("base")); + wrench.def(py::self -= py::self); + wrench.def(py::self += py::self); + wrench.def(py::self * double()); + wrench.def(double() * py::self); + wrench.def(py::self / double()); + wrench.def(py::self + py::self); + wrench.def(py::self - py::self); + wrench.def(py::self == py::self); + wrench.def(py::self != py::self); + wrench.def(py::hash(py::self)); + wrench.def("__neg__", [](const Wrench &w) + { + return operator-(w); + }, py::is_operator()); + wrench.def(py::pickle( + [](const Wrench &wr) + { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(wr.force, wr.torque); + }, + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) + throw std::runtime_error("Invalid state!"); + + /* Create a new C++ instance */ + Wrench wr(t[0].cast(), t[1].cast()); + return wr; + })); + + m.def("SetToZero", (void (*)(Wrench&)) &KDL::SetToZero, py::arg("wrench")); + m.def("Equal", (bool (*)(const Wrench&, const Wrench&, double eps)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + + + // -------------------- + // Twist + // -------------------- + py::class_ twist(m, "Twist"); + twist.def(py::init<>()); + twist.def(py::init(), py::arg("vel"), py::arg("rot")); + twist.def(py::init()); + twist.def_readwrite("vel", &Twist::vel); + twist.def_readwrite("rot", &Twist::rot); + twist.def("__getitem__", [](const Twist &t, int i) + { + if (i < 0 || i > 5) + throw py::index_error("Twist index out of range"); + + return t(i); + }, py::arg("index")); + twist.def("__setitem__", [](Twist &t, int i, double value) + { + if (i < 0 || i > 5) + throw py::index_error("Twist index out of range"); + + t(i) = value; + }, py::arg("index"), py::arg("value")); + twist.def("__repr__", [](const Twist &t) + { + std::ostringstream oss; + oss << t; + return oss.str(); + }); + twist.def("__copy__", [](const Twist& self) + { + return Twist(self); + }); + twist.def("__deepcopy__", [](const Twist& self, py::dict) + { + return Twist(self); + }, py::arg("memo")); + twist.def_static("Zero", &Twist::Zero); + twist.def("ReverseSign", &Twist::ReverseSign); + twist.def("RefPoint", &Twist::RefPoint, py::arg("base")); + twist.def(py::self -= py::self); + twist.def(py::self += py::self); + twist.def(py::self * double()); + twist.def(double() * py::self); + twist.def(py::self / double()); + twist.def(py::self + py::self); + twist.def(py::self - py::self); + twist.def(py::self == py::self); + twist.def(py::self != py::self); + twist.def(py::hash(py::self)); + twist.def("__neg__", [](const Twist &a) + { + return operator-(a); + }, py::is_operator()); + twist.def(py::pickle( + [](const Twist &tt) + { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(tt.vel, tt.rot); + }, + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) + throw std::runtime_error("Invalid state!"); + + /* Create a new C++ instance */ + Twist tt(t[0].cast(), t[1].cast()); + return tt; + })); + + m.def("dot", (double (*)(const Twist&, const Wrench&)) &KDL::dot); + m.def("dot", (double (*)(const Wrench&, const Twist&)) &KDL::dot); + m.def("SetToZero", (void (*)(Twist&)) &KDL::SetToZero, py::arg("twist")); + m.def("Equal", (bool (*)(const Twist&, const Twist&, double eps)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + + + // -------------------- + // Rotation + // -------------------- + py::class_ rotation(m, "Rotation"); + rotation.def(py::init<>()); + rotation.def(py::init(), + py::arg("xx"), py::arg("yx"), py::arg("zx"), + py::arg("xy"), py::arg("yy"), py::arg("zy"), + py::arg("xz"), py::arg("yz"), py::arg("zz")); + rotation.def(py::init(), + py::arg("x"), py::arg("y"), py::arg("z")); + rotation.def(py::init()); + rotation.def("__getitem__", [](const Rotation &r, std::tuple idx) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 2 || j < 0 || j > 2) + throw py::index_error("Rotation index out of range"); + + return r(i, j); + }, py::arg("index")); + rotation.def("__setitem__", [](Rotation &r, std::tuple idx, double value) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 2 || j < 0 || j > 2) + throw py::index_error("Rotation index out of range"); + + r(i, j) = value; + }, py::arg("index"), py::arg("value")); + rotation.def("__repr__", [](const Rotation &r) + { + std::ostringstream oss; + oss << r; + return oss.str(); + }); + rotation.def("__copy__", [](const Rotation& self) + { + return Rotation(self); + }); + rotation.def("__deepcopy__", [](const Rotation& self, py::dict) + { + return Rotation(self); + }, py::arg("memo")); + rotation.def("SetInverse", &Rotation::SetInverse); + rotation.def("Inverse", (Rotation (Rotation::*)(void) const) &Rotation::Inverse); + rotation.def("Inverse", (Vector (Rotation::*)(const Vector&) const) &Rotation::Inverse); + rotation.def("Inverse", (Wrench (Rotation::*)(const Wrench&) const) &Rotation::Inverse); + rotation.def("Inverse", (Twist (Rotation::*)(const Twist&) const) &Rotation::Inverse); + rotation.def_static("Identity", &Rotation::Identity); + rotation.def_static("RotX", &Rotation::RotX, py::arg("angle")); + rotation.def_static("RotY", &Rotation::RotY, py::arg("angle")); + rotation.def_static("RotZ", &Rotation::RotZ, py::arg("angle")); + rotation.def_static("Rot", &Rotation::Rot, py::arg("rotvec"), py::arg("angle")); + rotation.def_static("Rot2", &Rotation::Rot2, py::arg("rotvec"), py::arg("angle")); + rotation.def_static("EulerZYZ", &Rotation::EulerZYZ, py::arg("alpha"), py::arg("beta"), py::arg("gamma")); + rotation.def_static("RPY", &Rotation::RPY, py::arg("roll"), py::arg("pitch"), py::arg("yaw")); + rotation.def_static("EulerZYX", &Rotation::EulerZYX, py::arg("alpha"), py::arg("beta"), py::arg("gamma")); + rotation.def_static("Quaternion", &Rotation::Quaternion, py::arg("x"), py::arg("y"), py::arg("z"), py::arg("w")); + rotation.def("DoRotX", &Rotation::DoRotX, py::arg("angle")); + rotation.def("DoRotY", &Rotation::DoRotY, py::arg("angle")); + rotation.def("DoRotZ", &Rotation::DoRotZ, py::arg("angle")); + rotation.def("GetRot", &Rotation::GetRot); + rotation.def("GetRotAngle", [](const Rotation &r, double eps) + { + Vector axis; + double ret = r.GetRotAngle(axis, eps); + return py::make_tuple(ret, axis); + }, py::arg("eps") = epsilon); + rotation.def("GetEulerZYZ", [](const Rotation &r) + { + double Alfa, Beta, Gamma; + r.GetEulerZYZ(Alfa, Beta, Gamma); + return py::make_tuple(Alfa, Beta, Gamma); + }); + rotation.def("GetRPY", [](const Rotation &r) + { + double roll, pitch, yaw; + r.GetRPY(roll, pitch, yaw); + return py::make_tuple(roll, pitch, yaw); + }); + rotation.def("GetEulerZYX", [](const Rotation &r) + { + double Alfa, Beta, Gamma; + r.GetEulerZYX(Alfa, Beta, Gamma); + return py::make_tuple(Alfa, Beta, Gamma); + }); + rotation.def("GetQuaternion", [](const Rotation &r) + { + double x, y, z, w; + r.GetQuaternion(x, y, z, w); + return py::make_tuple(x, y, z, w); + }); + rotation.def("UnitX", (Vector (Rotation::*)() const) &Rotation::UnitX); + rotation.def("UnitY", (Vector (Rotation::*)() const) &Rotation::UnitY); + rotation.def("UnitZ", (Vector (Rotation::*)() const) &Rotation::UnitZ); + rotation.def("UnitX", (void (Rotation::*)(const Vector&)) &Rotation::UnitX, py::arg("vec")); + rotation.def("UnitY", (void (Rotation::*)(const Vector&)) &Rotation::UnitY, py::arg("vec")); + rotation.def("UnitZ", (void (Rotation::*)(const Vector&)) &Rotation::UnitZ, py::arg("vec")); + rotation.def(py::self * Vector()); + rotation.def(py::self * Twist()); + rotation.def(py::self * Wrench()); + rotation.def(py::self == py::self); + rotation.def(py::self != py::self); + rotation.def(py::self * py::self); + rotation.def(py::hash(py::self)); + rotation.def(py::pickle( + [](const Rotation &rot) + { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + double roll{0}, pitch{0}, yaw{0}; + rot.GetRPY(roll, pitch, yaw); + return py::make_tuple(roll, pitch, yaw); + }, + [](py::tuple t) + { // __setstate__ + if (t.size() != 3) + throw std::runtime_error("Invalid state!"); + + /* Create a new C++ instance */ + return Rotation::RPY(t[0].cast(), t[1].cast(), t[2].cast()); + })); + + m.def("Equal", (bool (*)(const Rotation&, const Rotation&, double eps)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + + + // -------------------- + // Frame + // -------------------- + py::class_ frame(m, "Frame"); + frame.def(py::init(), py::arg("R"), py::arg("V")); + frame.def(py::init()); + frame.def(py::init()); + frame.def(py::init()); + frame.def(py::init<>()); + frame.def_readwrite("M", &Frame::M); + frame.def_readwrite("p", &Frame::p); + frame.def("__getitem__", [](const Frame &frm, std::tuple idx) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 2 || j < 0 || j > 3) + throw py::index_error("Frame index out of range"); + + return frm(i, j); + }, py::arg("index")); + frame.def("__setitem__", [](Frame &frm, std::tuple idx, double value) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 2 || j < 0 || j > 3) + throw py::index_error("Frame index out of range"); + + if (j == 3) + frm.p(i) = value; + else + frm.M(i, j) = value; + }, py::arg("index"), py::arg("value")); + frame.def("__repr__", [](const Frame &frm) + { + std::ostringstream oss; + oss << frm; + return oss.str(); + }); + frame.def("__copy__", [](const Frame& self) + { + return Frame(self); + }); + frame.def("__deepcopy__", [](const Frame& self, py::dict) + { + return Frame(self); + }, py::arg("memo")); + frame.def_static("DH_Craig1989", &Frame::DH_Craig1989, py::arg("a"), py::arg("alpha"), py::arg("d"), py::arg("theta")); + frame.def_static("DH", &Frame::DH, py::arg("a"), py::arg("alpha"), py::arg("d"), py::arg("theta")); + frame.def("Inverse", (Frame (Frame::*)() const) &Frame::Inverse); + frame.def("Inverse", (Vector (Frame::*)(const Vector&) const) &Frame::Inverse); + frame.def("Inverse", (Wrench (Frame::*)(const Wrench&) const) &Frame::Inverse); + frame.def("Inverse", (Twist (Frame::*)(const Twist&) const) &Frame::Inverse); + frame.def_static("Identity", &Frame::Identity); + frame.def("Integrate", &Frame::Integrate, py::arg("twist"), py::arg("sample_frequency")); + frame.def(py::self * Vector()); + frame.def(py::self * Wrench()); + frame.def(py::self * Twist()); + frame.def(py::self * py::self); + frame.def(py::self == py::self); + frame.def(py::self != py::self); + frame.def(py::hash(py::self)); + frame.def(py::pickle( + [](const Frame &frm) + { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(frm.M, frm.p); + }, + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) + throw std::runtime_error("Invalid state!"); + + /* Create a new C++ instance */ + Frame frm(t[0].cast(), t[1].cast()); + return frm; + })); + + m.def("Equal", (bool (*)(const Frame&, const Frame&, double eps)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + + + // -------------------- + // Global + // -------------------- + m.def("diff", (Vector (*)(const Vector&, const Vector&, double dt)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt") = 1); + m.def("diff", (Vector (*)(const Rotation&, const Rotation&, double dt)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt") = 1); + m.def("diff", (Twist (*)(const Frame&, const Frame&, double dt)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt") = 1); + m.def("diff", (Twist (*)(const Twist&, const Twist&, double dt)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt") = 1); + m.def("diff", (Wrench (*)(const Wrench&, const Wrench&, double dt)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt") = 1); + m.def("addDelta", (Vector (*)(const Vector&, const Vector&, double dt)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt") = 1); + m.def("addDelta", (Rotation (*)(const Rotation&, const Vector&, double dt)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt") = 1); + m.def("addDelta", (Frame (*)(const Frame&, const Twist&, double dt)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt") = 1); + m.def("addDelta", (Twist (*)(const Twist&, const Twist&, double dt)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt") = 1); + m.def("addDelta", (Wrench (*)(const Wrench&, const Wrench&, double dt)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt") = 1); +} diff --git a/python_orocos_kdl/PyKDL/frames.sip b/python_orocos_kdl/PyKDL/frames.sip deleted file mode 100644 index 59f27ceef..000000000 --- a/python_orocos_kdl/PyKDL/frames.sip +++ /dev/null @@ -1,427 +0,0 @@ -//Copyright (C) 2007 Ruben Smits -// -//Version: 1.0 -//Author: Ruben Smits -//Maintainer: Ruben Smits -//URL: http://www.orocos.org/kdl -// -//This library is free software; you can redistribute it and/or -//modify it under the terms of the GNU Lesser General Public -//License as published by the Free Software Foundation; either -//version 2.1 of the License, or (at your option) any later version. -// -//This library is distributed in the hope that it will be useful, -//but WITHOUT ANY WARRANTY; without even the implied warranty of -//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -//Lesser General Public License for more details. -// -//You should have received a copy of the GNU Lesser General Public -//License along with this library; if not, write to the Free Software -//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA - - -class Vector{ - -%TypeHeaderCode -#include -#include -#include -using namespace KDL; -%End - -public: - Vector(); - Vector(double x, double y, double z); - Vector(const Vector& arg); - - void x(double); - void y(double); - void z(double); - - double x() const; - double y() const; - double z() const; - - double __getitem__ (int index) const; -%MethodCode - if (a0 < 0 || a0 > 2) { - PyErr_SetString(PyExc_IndexError, "Vector index out of range"); - return 0; - } - sipRes=(*sipCpp)(a0); -%End - - void __setitem__(int index, double value); -%MethodCode - if (a0 < 0 || a0 > 2) { - PyErr_SetString(PyExc_IndexError, "Vector index out of range"); - return 0; - } - (*sipCpp)(a0)=a1; -%End - - const std::string* __repr__() const; -%MethodCode - std::ostringstream oss; - oss<<(*sipCpp); - std::string s(oss.str()); - sipRes=&s; -%End - - void ReverseSign(); - - Vector& operator-=(const Vector& arg); - Vector& operator +=(const Vector& arg); - - static Vector Zero()/Factory/; - - double Norm(); - double Normalize(double eps=epsilon); - -%PickleCode - sipRes = Py_BuildValue("ddd", sipCpp->x(), sipCpp->y(), sipCpp->z()); -%End -}; - -void SetToZero(Vector& v); -Vector operator-(const Vector& arg)/Factory/; -Vector operator*(const Vector& lhs,double rhs)/Factory/; -Vector operator*(double lhs,const Vector& rhs)/Factory/; -Vector operator/(const Vector& lhs,double rhs)/Factory/; -Vector operator+(const Vector& lhs,const Vector& rhs)/Factory/; -Vector operator-(const Vector& lhs,const Vector& rhs)/Factory/; -Vector operator*(const Vector& lhs,const Vector& rhs)/Factory/; -double dot(const Vector& lhs,const Vector& rhs); -bool operator==(const Vector& a,const Vector& b); -bool operator!=(const Vector& a,const Vector& b); -bool Equal(const Vector& a,const Vector& b,double eps=epsilon); - -class Rotation{ - -%TypeHeaderCode -#include -#include -#include -using namespace KDL; -%End - -public: - Rotation(); - Rotation(double Xx,double Yx,double Zx, - double Xy,double Yy,double Zy, - double Xz,double Yz,double Zz); - Rotation(const Vector& x,const Vector& y,const Vector& z); - - double __getitem__(SIP_PYTUPLE) const; -%MethodCode - int i,j; - PyArg_ParseTuple(a0, "ii", &i, &j); - if (i < 0 || j < 0 || i > 2 || j > 2) { - PyErr_SetString(PyExc_IndexError, "Rotation index out of range"); - return 0; - } - sipRes=((const Rotation)(*sipCpp))(i,j); -%End - - void __setitem__(SIP_PYTUPLE,double value); -%MethodCode - int i,j; - PyArg_ParseTuple(a0,"ii",&i,&j); - if (i < 0 || j < 0 || i > 2 || j > 2) { - PyErr_SetString(PyExc_IndexError, "Rotation index out of range"); - return 0; - } - (*sipCpp)(i,j)=a1; -%End - - const std::string* __repr__() const; -%MethodCode - std::ostringstream oss; - oss<<(*sipCpp); - std::string s(oss.str()); - sipRes=&s; -%End - - void SetInverse(); - Rotation Inverse() const /Factory/; - Vector Inverse(const Vector& v) const /Factory/; - Wrench Inverse(const Wrench& w) const /Factory/; - Twist Inverse(const Twist& t) const /Factory/; - - static Rotation Identity()/Factory/; - static Rotation RotX(double angle)/Factory/; - static Rotation RotY(double angle)/Factory/; - static Rotation RotZ(double angle)/Factory/; - static Rotation Rot(const Vector& vec,double angle)/Factory/; - static Rotation Rot2(const Vector& vec,double angle)/Factory/; - static Rotation EulerZYZ(double Alfa,double Beta,double Gamma)/Factory/; - static Rotation RPY(double roll,double pitch,double yaw)/Factory/; - static Rotation EulerZYX(double Alfa,double Beta,double Gamma)/Factory/; - static Rotation Quaternion(double x, double y, double z, double w)/Factory/; - - - void DoRotX(double angle); - void DoRotY(double angle); - void DoRotZ(double angle); - - Vector GetRot() const /Factory/; - double GetRotAngle(Vector& axis /Out/,double eps=epsilon) const; - void GetEulerZYZ(double& alfa /Out/,double& beta /Out/,double& gamma /Out/) const; - void GetRPY(double& roll /Out/,double& pitch /Out/,double& yaw /Out/) const; - void GetEulerZYX(double& Alfa /Out/,double& Beta /Out/,double& Gamma /Out/) const; - void GetQuaternion(double& x /Out/,double& y /Out/,double& z /Out/, double& w) const; - - - Vector operator*(const Vector& v) const /Numeric,Factory/; - Twist operator*(const Twist& arg) const /Numeric,Factory/; - Wrench operator*(const Wrench& arg) const /Numeric,Factory/; - - Vector UnitX() const /Factory/; - Vector UnitY() const /Factory/; - Vector UnitZ() const /Factory/; - - void UnitX(const Vector& X); - void UnitY(const Vector& X); - void UnitZ(const Vector& X); - -%PickleCode - sipRes = Py_BuildValue("ddddddddd", (*sipCpp)(0,0), (*sipCpp)(0,1), (*sipCpp)(0,2), - (*sipCpp)(1,0), (*sipCpp)(1,1), (*sipCpp)(1,2), - (*sipCpp)(2,0), (*sipCpp)(2,1), (*sipCpp)(2,2)); -%End -}; -bool Equal(const Rotation& a,const Rotation& b,double eps=epsilon); -bool operator==(const Rotation& a,const Rotation& b); -bool operator!=(const Rotation& a,const Rotation& b); -Rotation operator *(const Rotation& lhs,const Rotation& rhs)/Factory/; - -class Frame{ - -%TypeHeaderCode -#include -#include -#include -using namespace KDL; -%End - -public: - Frame(const Rotation& R,const Vector& V); - Frame(const Vector& V); - Frame(const Rotation& R); - Frame(); - - Vector p; - Rotation M; - - double __getitem__ (SIP_PYTUPLE) const; -%MethodCode - int i,j; - PyArg_ParseTuple(a0,"ii",&i,&j); - if (i < 0 || j < 0 || i > 2 || j > 3) { - PyErr_SetString(PyExc_IndexError, "Frame index out of range"); - return 0; - } - sipRes=(*sipCpp)(i,j); -%End - - void __setitem__(SIP_PYTUPLE,double value); -%MethodCode - int i,j; - PyArg_ParseTuple(a0,"ii",&i,&j); - if (i < 0 || j < 0 || i > 2 || j > 3) { - PyErr_SetString(PyExc_IndexError, "Frame index out of range"); - return 0; - } - if(j==3) - (*sipCpp).p(i)=a1; - else - (*sipCpp).M(i,j)=a1; -%End - - const std::string* __repr__() const; -%MethodCode - std::ostringstream oss; - oss<<(*sipCpp); - std::string s(oss.str()); - sipRes=&s; -%End - - Frame DH_Craig1989(double a,double alpha,double d,double theta); - Frame DH(double a,double alpha,double d,double theta); - Frame Inverse()/Factory/; - Vector Inverse(const Vector& arg) const /Factory/; - Wrench Inverse(const Wrench& arg) const /Factory/; - Twist Inverse(const Twist& arg) const /Factory/; - - Vector operator*(const Vector& arg) const /Numeric,Factory/; - Wrench operator * (const Wrench& arg) const /Numeric,Factory/; - Twist operator * (const Twist& arg) const /Numeric,Factory/; - - static Frame Identity() /Factory/; - - void Integrate(const Twist& t_this,double frequency); - -%PickleCode - const sipTypeDef *vector_type = sipFindType("Vector"); - const sipTypeDef *rotation_type = sipFindType("Rotation"); - sipRes = Py_BuildValue("OO", sipConvertFromType(&(sipCpp->M), rotation_type, Py_None), - sipConvertFromType(&(sipCpp->p), vector_type, Py_None)); -%End -}; -Frame operator *(const Frame& lhs,const Frame& rhs)/Factory/; - -bool Equal(const Frame& a,const Frame& b,double eps=epsilon); -bool operator==(const Frame& a,const Frame& b); -bool operator!=(const Frame& a,const Frame& b); - -class Twist -{ - -%TypeHeaderCode -#include -#include -#include -using namespace KDL; -%End -public: - Vector vel; - Vector rot; - - Twist(); - Twist(const Vector& _vel,const Vector& _rot); - - Twist& operator-=(const Twist& arg); - Twist& operator+=(const Twist& arg); - - double __getitem__ (int i) const; -%MethodCode - if (a0 < 0 || a0 > 5) { - PyErr_SetString(PyExc_IndexError, "Twist index out of range"); - return 0; - } - sipRes=(*sipCpp)(a0); -%End - - void __setitem__(int i, double value); -%MethodCode - if (a0 < 0 || a0 > 5) { - PyErr_SetString(PyExc_IndexError, "Twist index out of range"); - return 0; - } - (*sipCpp)(a0)=a1; -%End - - const std::string* __repr__() const; -%MethodCode - std::ostringstream oss; - oss<<(*sipCpp); - std::string s(oss.str()); - sipRes=&s; -%End - - static Twist Zero() /Factory/; - void ReverseSign(); - - Twist RefPoint(const Vector& v_base_AB) const /Factory/; - -%PickleCode - const sipTypeDef *vector_type = sipFindType("Vector"); - sipRes = Py_BuildValue("OO", sipConvertFromType(&(sipCpp->vel), vector_type, Py_None), - sipConvertFromType(&(sipCpp->rot), vector_type, Py_None)); -%End -}; - -Twist operator*(const Twist& lhs,double rhs)/Factory/; -Twist operator*(double lhs,const Twist& rhs)/Factory/; -Twist operator/(const Twist& lhs,double rhs)/Factory/; -Twist operator+(const Twist& lhs,const Twist& rhs)/Factory/; -Twist operator-(const Twist& lhs,const Twist& rhs)/Factory/; -Twist operator-(const Twist& arg)/Factory/; -double dot(const Twist& lhs,const Wrench& rhs); -double dot(const Wrench& rhs,const Twist& lhs); -void SetToZero(Twist& v); - -bool Equal(const Twist& a,const Twist& b,double eps=epsilon); -bool operator==(const Twist& a,const Twist& b); -bool operator!=(const Twist& a,const Twist& b); - -class Wrench -{ - -%TypeHeaderCode -#include -#include -#include -using namespace KDL; -%End - -public: - Vector force; - Vector torque; - - Wrench(); - Wrench(const Vector& force,const Vector& torque); - - Wrench& operator-=(const Wrench& arg); - Wrench& operator+=(const Wrench& arg); - - - double __getitem__ (int i) const; -%MethodCode - if (a0 < 0 || a0 > 5) { - PyErr_SetString(PyExc_IndexError, "Wrench index out of range"); - return 0; - } - sipRes=(*sipCpp)(a0); -%End - - void __setitem__(int i, double value); -%MethodCode - if (a0 < 0 || a0 > 5) { - PyErr_SetString(PyExc_IndexError, "Wrench index out of range"); - return 0; - } - (*sipCpp)(a0)=a1; -%End - - const std::string* __repr__() const; -%MethodCode - std::ostringstream oss; - oss<<(*sipCpp); - std::string s(oss.str()); - sipRes=&s; -%End - - static Wrench Zero() /Factory/; - void ReverseSign(); - Wrench RefPoint(const Vector& v_base_AB) const /Factory/; - -%PickleCode - const sipTypeDef *vector_type = sipFindType("Vector"); - sipRes = Py_BuildValue("OO", sipConvertFromType(&(sipCpp->force), vector_type, Py_None), - sipConvertFromType(&(sipCpp->torque), vector_type, Py_None)); -%End -}; - -Wrench operator*(const Wrench& lhs,double rhs) /Factory/; -Wrench operator*(double lhs,const Wrench& rhs) /Factory/; -Wrench operator/(const Wrench& lhs,double rhs) /Factory/; -Wrench operator+(const Wrench& lhs,const Wrench& rhs) /Factory/; -Wrench operator-(const Wrench& lhs,const Wrench& rhs) /Factory/; -Wrench operator-(const Wrench& arg) /Factory/; -void SetToZero(Wrench& v); -bool Equal(const Wrench& a,const Wrench& b,double eps=epsilon); -bool operator==(const Wrench& a,const Wrench& b); -bool operator!=(const Wrench& a,const Wrench& b); - -Vector diff(const Vector& a,const Vector& b,double dt=1)/Factory/; -Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt=1)/Factory/; -Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt=1)/Factory/; -Twist diff(const Twist& a,const Twist& b,double dt=1)/Factory/; -Wrench diff(const Wrench& W_a_p1,const Wrench& W_a_p2,double dt=1)/Factory/; -Vector addDelta(const Vector& a,const Vector&da,double dt=1)/Factory/; -Rotation addDelta(const Rotation& a,const Vector&da,double dt=1)/Factory/; -Frame addDelta(const Frame& a,const Twist& da,double dt=1)/Factory/; -Twist addDelta(const Twist& a,const Twist&da,double dt=1)/Factory/; -Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1)/Factory/; - diff --git a/python_orocos_kdl/PyKDL/framevel.cpp b/python_orocos_kdl/PyKDL/framevel.cpp new file mode 100644 index 000000000..b8518a066 --- /dev/null +++ b/python_orocos_kdl/PyKDL/framevel.cpp @@ -0,0 +1,425 @@ +//Copyright (C) 2007 Ruben Smits +// +//Version: 1.0 +//Author: Ruben Smits Ruben Smits +//Author: Zihan Chen +//Author: Matthijs van der Burgh +//Maintainer: Ruben Smits Ruben Smits +//Maintainer: Matthijs van der Burgh +//URL: http://www.orocos.org/kdl +// +//This library is free software; you can redistribute it and/or +//modify it under the terms of the GNU Lesser General Public +//License as published by the Free Software Foundation; either +//version 2.1 of the License, or (at your option) any later version. +// +//This library is distributed in the hope that it will be useful, +//but WITHOUT ANY WARRANTY; without even the implied warranty of +//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +//Lesser General Public License for more details. +// +//You should have received a copy of the GNU Lesser General Public +//License along with this library; if not, write to the Free Software +//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +#include +#include +#include "PyKDL.h" + +namespace py = pybind11; +using namespace KDL; + + +void init_framevel(pybind11::module &m) +{ + // -------------------- + // doubleVel + // -------------------- + py::class_ double_vel(m, "doubleVel"); + double_vel.def(py::init<>()); + double_vel.def(py::init()); + double_vel.def(py::init()); + double_vel.def(py::init()); + double_vel.def_readwrite("t", &doubleVel::t); + double_vel.def_readwrite("grad", &doubleVel::grad); + double_vel.def("value", &doubleVel::value); + double_vel.def("deriv", &doubleVel::deriv); + double_vel.def("__repr__", [](const doubleVel &d) + { + std::ostringstream oss; + oss << d; + return oss.str(); + }); + double_vel.def("__copy__", [](const doubleVel& self) + { + return doubleVel(self); + }); + double_vel.def("__deepcopy__", [](const doubleVel& self, py::dict) + { + return doubleVel(self); + }, py::arg("memo")); + + double_vel.def(py::self == py::self); + double_vel.def(py::self != py::self); + double_vel.def(py::hash(py::self)); + double_vel.def("__neg__", [](const doubleVel &a) + { + return operator-(a); + }, py::is_operator()); + + m.def("diff", (doubleVel (*)(const doubleVel&, const doubleVel&, double)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt")=1.0); + m.def("addDelta", (doubleVel (*)(const doubleVel&, const doubleVel&, double)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt")=1.0); + m.def("Equal", (bool (*)(const doubleVel&, const doubleVel&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + + + // -------------------- + // VectorVel + // -------------------- + py::class_ vector_vel(m, "VectorVel"); + vector_vel.def_readwrite("p", &VectorVel::p); + vector_vel.def_readwrite("v", &VectorVel::v); + vector_vel.def(py::init<>()); + vector_vel.def(py::init(), py::arg("p"), py::arg("v")); + vector_vel.def(py::init(), py::arg("p")); + vector_vel.def(py::init()); + vector_vel.def("value", &VectorVel::value); + vector_vel.def("deriv", &VectorVel::deriv); + vector_vel.def("__repr__", [](const VectorVel &vv) + { + std::ostringstream oss; + oss << vv; + return oss.str(); + }); + vector_vel.def("__copy__", [](const VectorVel& self) + { + return VectorVel(self); + }); + vector_vel.def("__deepcopy__", [](const VectorVel& self, py::dict) + { + return VectorVel(self); + }, py::arg("memo")); + vector_vel.def_static("Zero", &VectorVel::Zero); + vector_vel.def("ReverseSign", &VectorVel::ReverseSign); + vector_vel.def("Norm", &VectorVel::Norm, py::arg("eps")=epsilon); + vector_vel.def(py::self += py::self); + vector_vel.def(py::self -= py::self); + vector_vel.def(py::self + py::self); + vector_vel.def(py::self - py::self); + vector_vel.def(Vector() + py::self); + vector_vel.def(Vector() - py::self); + vector_vel.def(py::self + Vector()); + vector_vel.def(py::self - Vector()); + + vector_vel.def(py::self * py::self); + vector_vel.def(py::self * Vector()); + vector_vel.def(Vector() * py::self); + vector_vel.def(double() * py::self); + vector_vel.def(py::self * double()); + vector_vel.def(doubleVel() * py::self); + vector_vel.def(py::self * doubleVel()); + vector_vel.def(Rotation() * py::self); + + vector_vel.def(py::self / double()); + vector_vel.def(py::self / doubleVel()); + + vector_vel.def(py::self == py::self); + vector_vel.def(py::self != py::self); + vector_vel.def(Vector() == py::self); + vector_vel.def(Vector() != py::self); + vector_vel.def(py::self == Vector()); + vector_vel.def(py::self != Vector()); + vector_vel.def(py::hash(py::self)); + vector_vel.def("__neg__", [](const VectorVel &a) + { + return operator-(a); + }, py::is_operator()); + vector_vel.def(py::pickle( + [](const VectorVel &vv) + { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(vv.p, vv.v); + }, + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) + throw std::runtime_error("Invalid state!"); + + /* Create a new C++ instance */ + VectorVel vv(t[0].cast(), t[1].cast()); + return vv; + })); + + m.def("SetToZero", (void (*)(VectorVel&)) &KDL::SetToZero, py::arg("vector_vel")); + m.def("Equal", (bool (*)(const VectorVel&, const VectorVel&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const Vector&, const VectorVel&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const VectorVel&, const Vector&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + + m.def("dot", (doubleVel (*)(const VectorVel&, const VectorVel&)) &KDL::dot); + m.def("dot", (doubleVel (*)(const VectorVel&, const Vector&)) &KDL::dot); + m.def("dot", (doubleVel (*)(const Vector&, const VectorVel&)) &KDL::dot); + + + // -------------------- + // TwistVel + // -------------------- + py::class_ twist_vel(m, "TwistVel"); + twist_vel.def_readwrite("vel", &TwistVel::vel); + twist_vel.def_readwrite("rot", &TwistVel::rot); + twist_vel.def(py::init<>()); + twist_vel.def(py::init(), py::arg("vel"), py::arg("rot")); + twist_vel.def(py::init(), py::arg("p"), py::arg("v")); + twist_vel.def(py::init(), py::arg("p")); + twist_vel.def(py::init()); + twist_vel.def("value", &TwistVel::value); + twist_vel.def("deriv", &TwistVel::deriv); + twist_vel.def("__repr__", [](const TwistVel &tv) + { + std::ostringstream oss; + oss << tv; + return oss.str(); + }); + twist_vel.def("__copy__", [](const TwistVel& self) + { + return TwistVel(self); + }); + twist_vel.def("__deepcopy__", [](const TwistVel& self, py::dict) + { + return TwistVel(self); + }, py::arg("memo")); + twist_vel.def_static("Zero", &TwistVel::Zero); + twist_vel.def("ReverseSign", &TwistVel::ReverseSign); + twist_vel.def("RefPoint", &TwistVel::RefPoint, py::arg("base")); + twist_vel.def("GetTwist", &TwistVel::GetTwist); + twist_vel.def("GetTwistDot", &TwistVel::GetTwistDot); + + twist_vel.def(py::self -= py::self); + twist_vel.def(py::self += py::self); + twist_vel.def(py::self * double()); + twist_vel.def(double() * py::self); + twist_vel.def(py::self / double()); + + twist_vel.def(py::self * doubleVel()); + twist_vel.def(doubleVel() * py::self); + twist_vel.def(py::self / doubleVel()); + + twist_vel.def(py::self + py::self); + twist_vel.def(py::self - py::self); + + twist_vel.def(py::self == py::self); + twist_vel.def(py::self != py::self); + twist_vel.def(Twist() == py::self); + twist_vel.def(Twist() != py::self); + twist_vel.def(py::self == Twist()); + twist_vel.def(py::self != Twist()); + twist_vel.def(py::hash(py::self)); + twist_vel.def("__neg__", [](const TwistVel &a) + { + return operator-(a); + }, py::is_operator()); + twist_vel.def(py::pickle( + [](const TwistVel &tv) + { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(tv.vel, tv.rot); + }, + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) + throw std::runtime_error("Invalid state!"); + + /* Create a new C++ instance */ + TwistVel tv(t[0].cast(), t[1].cast()); + return tv; + })); + + m.def("SetToZero", (void (*)(TwistVel&)) &KDL::SetToZero, py::arg("twist_vel")); + m.def("Equal", (bool (*)(const TwistVel&, const TwistVel&, double)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const Twist&, const TwistVel&, double)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const TwistVel&, const Twist&, double)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + + + // -------------------- + // RotationVel + // -------------------- + py::class_ rotation_vel(m, "RotationVel"); + rotation_vel.def_readwrite("R", &RotationVel::R); + rotation_vel.def_readwrite("w", &RotationVel::w); + rotation_vel.def(py::init<>()); + rotation_vel.def(py::init(), py::arg("R")); + rotation_vel.def(py::init(), py::arg("R"), py::arg("w")); + rotation_vel.def(py::init()); + rotation_vel.def("value", &RotationVel::value); + rotation_vel.def("deriv", &RotationVel::deriv); + rotation_vel.def("__repr__", [](const RotationVel &rv) + { + std::ostringstream oss; + oss << rv; + return oss.str(); + }); + rotation_vel.def("__copy__", [](const RotationVel& self) + { + return RotationVel(self); + }); + rotation_vel.def("__deepcopy__", [](const RotationVel& self, py::dict) + { + return RotationVel(self); + }, py::arg("memo")); + rotation_vel.def("UnitX", &RotationVel::UnitX); + rotation_vel.def("UnitY", &RotationVel::UnitY); + rotation_vel.def("UnitZ", &RotationVel::UnitZ); + rotation_vel.def_static("Identity", &RotationVel::Identity); + rotation_vel.def("Inverse", (RotationVel (RotationVel::*)(void) const) &RotationVel::Inverse); + rotation_vel.def("Inverse", (VectorVel (RotationVel::*)(const VectorVel&) const) &RotationVel::Inverse); + rotation_vel.def("Inverse", (VectorVel (RotationVel::*)(const Vector&) const) &RotationVel::Inverse); + rotation_vel.def("DoRotX", &RotationVel::DoRotX, py::arg("angle")); + rotation_vel.def("DoRotY", &RotationVel::DoRotY, py::arg("angle")); + rotation_vel.def("DoRotZ", &RotationVel::DoRotZ, py::arg("angle")); + rotation_vel.def_static("RotX", &RotationVel::RotX, py::arg("angle")); + rotation_vel.def_static("RotY", &RotationVel::RotY, py::arg("angle")); + rotation_vel.def_static("RotZ", &RotationVel::RotZ, py::arg("angle")); + rotation_vel.def_static("Rot", &RotationVel::Rot, py::arg("rotvec"), py::arg("angle")); + rotation_vel.def_static("Rot2", &RotationVel::Rot2, py::arg("rotvec"), py::arg("angle")); + + rotation_vel.def("Inverse", (TwistVel (RotationVel::*)(const TwistVel&) const) &RotationVel::Inverse); + rotation_vel.def("Inverse", (TwistVel (RotationVel::*)(const Twist&) const) &RotationVel::Inverse); + + rotation_vel.def(py::self * VectorVel()); + rotation_vel.def(py::self * Vector()); + rotation_vel.def(py::self * TwistVel()); + rotation_vel.def(py::self * Twist()); + rotation_vel.def(py::self * py::self); + rotation_vel.def(Rotation() * py::self); + rotation_vel.def(py::self * Rotation()); + + rotation_vel.def(py::self == py::self); + rotation_vel.def(py::self != py::self); + rotation_vel.def(Rotation() == py::self); + rotation_vel.def(Rotation() != py::self); + rotation_vel.def(py::self == Rotation()); + rotation_vel.def(py::self != Rotation()); + rotation_vel.def(py::hash(py::self)); + rotation_vel.def(py::pickle( + [](const RotationVel &rv) + { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(rv.R, rv.w); + }, + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) + throw std::runtime_error("Invalid state!"); + + /* Create a new C++ instance */ + RotationVel rv(t[0].cast(), t[1].cast()); + return rv; + })); + + m.def("Equal", (bool (*)(const RotationVel&, const RotationVel&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const Rotation&, const RotationVel&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const RotationVel&, const Rotation&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + + + // -------------------- + // FrameVel + // -------------------- + py::class_ frame_vel(m, "FrameVel"); + frame_vel.def_readwrite("M", &FrameVel::M); + frame_vel.def_readwrite("p", &FrameVel::p); + frame_vel.def(py::init<>()); + frame_vel.def(py::init(), py::arg("T")); + frame_vel.def(py::init(), py::arg("T"), py::arg("t")); + frame_vel.def(py::init(), py::arg("M"), py::arg("p")); + frame_vel.def(py::init()); + frame_vel.def("value", &FrameVel::value); + frame_vel.def("deriv", &FrameVel::deriv); + frame_vel.def("__repr__", [](const FrameVel &fv) + { + std::ostringstream oss; + oss << fv; + return oss.str(); + }); + frame_vel.def("__copy__", [](const FrameVel& self) + { + return FrameVel(self); + }); + frame_vel.def("__deepcopy__", [](const FrameVel& self, py::dict) + { + return FrameVel(self); + }, py::arg("memo")); + frame_vel.def_static("Identity", &FrameVel::Identity); + frame_vel.def("Inverse", (FrameVel (FrameVel::*)() const) &FrameVel::Inverse); + frame_vel.def("Inverse", (VectorVel (FrameVel::*)(const VectorVel&) const) &FrameVel::Inverse); + frame_vel.def("Inverse", (VectorVel (FrameVel::*)(const Vector&) const) &FrameVel::Inverse); + frame_vel.def(py::self * VectorVel()); + frame_vel.def(py::self * Vector()); + frame_vel.def("GetFrame", &FrameVel::GetFrame); + frame_vel.def("GetTwist", &FrameVel::GetTwist); + frame_vel.def("Inverse", (TwistVel (FrameVel::*)(const TwistVel&) const) &FrameVel::Inverse); + frame_vel.def("Inverse", (TwistVel (FrameVel::*)(const Twist&) const) &FrameVel::Inverse); + frame_vel.def(py::self * TwistVel()); + frame_vel.def(py::self * Twist()); + frame_vel.def(py::self * py::self); + frame_vel.def(Frame() * py::self); + frame_vel.def(py::self * Frame()); + + frame_vel.def(py::self == py::self); + frame_vel.def(py::self != py::self); + frame_vel.def(Frame() == py::self); + frame_vel.def(Frame() != py::self); + frame_vel.def(py::self == Frame()); + frame_vel.def(py::self != Frame()); + frame_vel.def(py::hash(py::self)); + frame_vel.def(py::pickle( + [](const FrameVel &fv) + { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(fv.M, fv.p); + }, + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) + throw std::runtime_error("Invalid state!"); + + /* Create a new C++ instance */ + FrameVel rv(t[0].cast(), t[1].cast()); + return rv; + })); + + m.def("Equal", (bool (*)(const FrameVel&, const FrameVel&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const Frame&, const FrameVel&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const FrameVel&, const Frame&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + + + // -------------------- + // Global + // -------------------- + m.def("diff", (VectorVel (*)(const VectorVel&, const VectorVel&, double)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt")=1.0); + m.def("diff", (VectorVel (*)(const RotationVel&, const RotationVel&, double)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt")=1.0); + m.def("diff", (TwistVel (*)(const FrameVel&, const FrameVel&, double)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt")=1.0); + + m.def("addDelta", (VectorVel (*)(const VectorVel&, const VectorVel&, double)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt")=1.0); + m.def("addDelta", (RotationVel (*)(const RotationVel&, const VectorVel&, double)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt")=1.0); + m.def("addDelta", (FrameVel (*)(const FrameVel&, const TwistVel&, double)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt")=1.0); +} diff --git a/python_orocos_kdl/PyKDL/framevel.sip b/python_orocos_kdl/PyKDL/framevel.sip deleted file mode 100644 index a3e0ad0ac..000000000 --- a/python_orocos_kdl/PyKDL/framevel.sip +++ /dev/null @@ -1,261 +0,0 @@ -//Copyright (C) 2007 Ruben Smits -// -//Version: 1.0 -//Author: Ruben Smits -//Maintainer: Ruben Smits -//URL: http://www.orocos.org/kdl -// -//This library is free software; you can redistribute it and/or -//modify it under the terms of the GNU Lesser General Public -//License as published by the Free Software Foundation; either -//version 2.1 of the License, or (at your option) any later version. -// -//This library is distributed in the hope that it will be useful, -//but WITHOUT ANY WARRANTY; without even the implied warranty of -//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -//Lesser General Public License for more details. -// -//You should have received a copy of the GNU Lesser General Public -//License along with this library; if not, write to the Free Software -//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA - -class doubleVel -{ - -%TypeHeaderCode -#include -using namespace KDL; -%End -public: - typedef Rall1d doubleVel; - double t; - double grad; -}; - -doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0); -doubleVel addDelta(const doubleVel& a,const doubleVel&da,double dt=1.0); -bool Equal(const doubleVel& r1,const doubleVel& r2,double eps=epsilon); -//bool Equal(const double& r1,const doubleVel& r2,double eps=epsilon); -//bool Equal(const doubleVel& r1,const double& r2,double eps=epsilon); - -class VectorVel -{ -%TypeHeaderCode -#include -using namespace KDL; -%End -public: - Vector p; - Vector v; - - VectorVel(); - VectorVel(const Vector& _p,const Vector& _v); - VectorVel(const Vector& _p); - Vector value() const; - Vector deriv() const; - - VectorVel& operator += (const VectorVel& arg); - VectorVel& operator -= (const VectorVel& arg); - static VectorVel Zero(); - void ReverseSign(); - doubleVel Norm() const; - -%PickleCode - const sipTypeDef *vector_type = sipFindType("Vector"); - sipRes = Py_BuildValue("OO", sipConvertFromType(&(sipCpp->p), vector_type, Py_None), - sipConvertFromType(&(sipCpp->v), vector_type, Py_None)); -%End -}; - -VectorVel operator + (const VectorVel& r1,const VectorVel& r2); -VectorVel operator - (const VectorVel& r1,const VectorVel& r2); -VectorVel operator + (const Vector& r1,const VectorVel& r2); -VectorVel operator - (const Vector& r1,const VectorVel& r2); -VectorVel operator + (const VectorVel& r1,const Vector& r2); -VectorVel operator - (const VectorVel& r1,const Vector& r2); -VectorVel operator * (const VectorVel& r1,const VectorVel& r2); -VectorVel operator * (const VectorVel& r1,const Vector& r2); -VectorVel operator * (const Vector& r1,const VectorVel& r2); -VectorVel operator * (const VectorVel& r1,double r2); -VectorVel operator * (double r1,const VectorVel& r2); -VectorVel operator * (const doubleVel& r1,const VectorVel& r2); -VectorVel operator * (const VectorVel& r2,const doubleVel& r1); -VectorVel operator*(const Rotation& R,const VectorVel& x); - -VectorVel operator / (const VectorVel& r1,double r2); -VectorVel operator / (const VectorVel& r2,const doubleVel& r1); - -bool Equal(const VectorVel& r1,const VectorVel& r2,double eps=epsilon); -bool Equal(const Vector& r1,const VectorVel& r2,double eps=epsilon); -bool Equal(const VectorVel& r1,const Vector& r2,double eps=epsilon); - -VectorVel operator - (const VectorVel& r); -doubleVel dot(const VectorVel& lhs,const VectorVel& rhs); -doubleVel dot(const VectorVel& lhs,const Vector& rhs); -doubleVel dot(const Vector& lhs,const VectorVel& rhs); - -class RotationVel -{ -%TypeHeaderCode -#include -using namespace KDL; -%End - -public: - Rotation R; - Vector w; - RotationVel(); - RotationVel(const Rotation& _R); - RotationVel(const Rotation& _R,const Vector& _w); - - Rotation value() const; - Vector deriv() const; - - - VectorVel UnitX() const; - VectorVel UnitY() const; - VectorVel UnitZ() const; - static RotationVel Identity(); - RotationVel Inverse() const; - VectorVel Inverse(const VectorVel& arg) const; - VectorVel Inverse(const Vector& arg) const; - VectorVel operator*(const VectorVel& arg) const; - VectorVel operator*(const Vector& arg) const; - void DoRotX(const doubleVel& angle); - void DoRotY(const doubleVel& angle); - void DoRotZ(const doubleVel& angle); - static RotationVel RotX(const doubleVel& angle); - static RotationVel RotY(const doubleVel& angle); - static RotationVel RotZ(const doubleVel& angle); - static RotationVel Rot(const Vector& rotvec,const doubleVel& angle); - static RotationVel Rot2(const Vector& rotvec,const doubleVel& angle); - - TwistVel Inverse(const TwistVel& arg) const; - TwistVel Inverse(const Twist& arg) const; - TwistVel operator * (const TwistVel& arg) const; - TwistVel operator * (const Twist& arg) const; - -%PickleCode - const sipTypeDef *vector_type = sipFindType("Vector"); - const sipTypeDef *rotation_type = sipFindType("Rotation"); - sipRes = Py_BuildValue("OO", sipConvertFromType(&(sipCpp->R), rotation_type, Py_None), - sipConvertFromType(&(sipCpp->w), vector_type, Py_None)); -%End -}; -RotationVel operator* (const RotationVel& r1,const RotationVel& r2); -RotationVel operator* (const Rotation& r1,const RotationVel& r2); -RotationVel operator* (const RotationVel& r1,const Rotation& r2); -bool Equal(const RotationVel& r1,const RotationVel& r2,double eps=epsilon); -bool Equal(const Rotation& r1,const RotationVel& r2,double eps=epsilon); -bool Equal(const RotationVel& r1,const Rotation& r2,double eps=epsilon); - - - - -class FrameVel -{ - -%TypeHeaderCode -#include -using namespace KDL; -%End - -public: - RotationVel M; - VectorVel p; - - FrameVel(); - FrameVel(const Frame& _T); - FrameVel(const Frame& _T,const Twist& _t); - FrameVel(const RotationVel& _M,const VectorVel& _p); - - Frame value() const; - Twist deriv() const; - - - static FrameVel Identity(); - FrameVel Inverse() const; - VectorVel Inverse(const VectorVel& arg) const; - VectorVel operator*(const VectorVel& arg) const; - VectorVel operator*(const Vector& arg) const; - VectorVel Inverse(const Vector& arg) const; - Frame GetFrame() const; - Twist GetTwist() const; - - TwistVel Inverse(const TwistVel& arg) const; - TwistVel Inverse(const Twist& arg) const; - TwistVel operator * (const TwistVel& arg) const; - TwistVel operator * (const Twist& arg) const; - -%PickleCode - const sipTypeDef *vectorvel_type = sipFindType("VectorVel"); - const sipTypeDef *rotationvel_type = sipFindType("RotationVel"); - sipRes = Py_BuildValue("OO", sipConvertFromType(&(sipCpp->M), rotationvel_type, Py_None), - sipConvertFromType(&(sipCpp->p), vectorvel_type, Py_None)); -%End -}; -FrameVel operator * (const FrameVel& f1,const FrameVel& f2); -FrameVel operator * (const Frame& f1,const FrameVel& f2); -FrameVel operator * (const FrameVel& f1,const Frame& f2); -bool Equal(const FrameVel& r1,const FrameVel& r2,double eps=epsilon); -bool Equal(const Frame& r1,const FrameVel& r2,double eps=epsilon); -bool Equal(const FrameVel& r1,const Frame& r2,double eps=epsilon); - -class TwistVel -{ -%TypeHeaderCode -#include -using namespace KDL; -%End - -public: - VectorVel vel; - VectorVel rot; - - TwistVel(); - TwistVel(const VectorVel& _vel,const VectorVel& _rot); - TwistVel(const Twist& p,const Twist& v); - TwistVel(const Twist& p); - - Twist value() const; - Twist deriv() const; - TwistVel& operator-=(const TwistVel& arg); - TwistVel& operator+=(const TwistVel& arg); - - static TwistVel Zero(); - - void ReverseSign(); - - TwistVel RefPoint(const VectorVel& v_base_AB); - Twist GetTwist() const; - Twist GetTwistDot() const; -%PickleCode - const sipTypeDef *vectorvel_type = sipFindType("VectorVel"); - sipRes = Py_BuildValue("OO", sipConvertFromType(&(sipCpp->vel), vectorvel_type, Py_None), - sipConvertFromType(&(sipCpp->rot), vectorvel_type, Py_None)); -%End -}; -TwistVel operator*(const TwistVel& lhs,double rhs); -TwistVel operator*(double lhs,const TwistVel& rhs); -TwistVel operator/(const TwistVel& lhs,double rhs); - -TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs); -TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs); -TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs); - -TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs); -TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs); -TwistVel operator-(const TwistVel& arg); -void SetToZero(TwistVel& v); -bool Equal(const TwistVel& a,const TwistVel& b,double eps=epsilon); -bool Equal(const Twist& a,const TwistVel& b,double eps=epsilon); -bool Equal(const TwistVel& a,const Twist& b,double eps=epsilon); - - -VectorVel diff(const VectorVel& a,const VectorVel& b,double dt=1.0); -VectorVel addDelta(const VectorVel& a,const VectorVel&da,double dt=1.0); -VectorVel diff(const RotationVel& a,const RotationVel& b,double dt = 1.0); -RotationVel addDelta(const RotationVel& a,const VectorVel&da,double dt=1.0); -TwistVel diff(const FrameVel& a,const FrameVel& b,double dt=1.0); -FrameVel addDelta(const FrameVel& a,const TwistVel& da,double dt=1.0); - diff --git a/python_orocos_kdl/PyKDL/kinfam.cpp b/python_orocos_kdl/PyKDL/kinfam.cpp new file mode 100644 index 000000000..3e6c5d2d1 --- /dev/null +++ b/python_orocos_kdl/PyKDL/kinfam.cpp @@ -0,0 +1,516 @@ +//Copyright (C) 2007 Ruben Smits +// +//Version: 1.0 +//Author: Ruben Smits Ruben Smits +//Author: Zihan Chen +//Author: Matthijs van der Burgh +//Maintainer: Ruben Smits Ruben Smits +//Maintainer: Matthijs van der Burgh +//URL: http://www.orocos.org/kdl +// +//This library is free software; you can redistribute it and/or +//modify it under the terms of the GNU Lesser General Public +//License as published by the Free Software Foundation; either +//version 2.1 of the License, or (at your option) any later version. +// +//This library is distributed in the hope that it will be useful, +//but WITHOUT ANY WARRANTY; without even the implied warranty of +//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +//Lesser General Public License for more details. +// +//You should have received a copy of the GNU Lesser General Public +//License along with this library; if not, write to the Free Software +//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "PyKDL.h" + +namespace py = pybind11; +using namespace KDL; + + +void init_kinfam(pybind11::module &m) +{ + // -------------------- + // Joint + // -------------------- + py::class_ joint(m, "Joint"); + py::enum_ joint_type(joint, "JointType"); + joint_type.value("RotAxis", Joint::JointType::RotAxis); + joint_type.value("RotX", Joint::JointType::RotX); + joint_type.value("RotY", Joint::JointType::RotY); + joint_type.value("RotZ", Joint::JointType::RotZ); + joint_type.value("TransAxis", Joint::JointType::TransAxis); + joint_type.value("TransX", Joint::JointType::TransX); + joint_type.value("TransY", Joint::JointType::TransY); + joint_type.value("TransZ", Joint::JointType::TransZ); + joint_type.value("Fixed", Joint::JointType::Fixed); + joint_type.export_values(); + + joint.def(py::init<>()); + joint.def(py::init(), + py::arg("name"), py::arg("type")=Joint::JointType::Fixed, py::arg("scale")=1, py::arg("offset")=0, + py::arg("inertia")=0, py::arg("damping")=0, py::arg("stiffness")=0); + joint.def(py::init(), + py::arg("type")=Joint::JointType::Fixed, py::arg("scale")=1, py::arg("offset")=0, + py::arg("inertia")=0, py::arg("damping")=0, py::arg("stiffness")=0); + joint.def(py::init(), + py::arg("name"), py::arg("origin"), py::arg("axis"), py::arg("type"), py::arg("scale")=1, py::arg("offset")=0, + py::arg("inertia")=0, py::arg("damping")=0, py::arg("stiffness")=0); + joint.def(py::init(), + py::arg("origin"), py::arg("axis"), py::arg("type"), py::arg("scale")=1, py::arg("offset")=0, + py::arg("inertia")=0, py::arg("damping")=0, py::arg("stiffness")=0); + joint.def(py::init()); + joint.def("pose", &Joint::pose, py::arg("q")); + joint.def("twist", &Joint::twist, py::arg("q_dot")); + joint.def("JointAxis", &Joint::JointAxis); + joint.def("JointOrigin", &Joint::JointOrigin); + joint.def("getName", &Joint::getName); + joint.def("getType", &Joint::getType); + joint.def("getTypeName", &Joint::getTypeName); + joint.def("__repr__", [](const Joint &j) + { + std::ostringstream oss; + oss << j; + return oss.str(); + }); + + + // -------------------- + // RotationalInertia + // -------------------- + py::class_ rotational_inertia(m, "RotationalInertia"); + rotational_inertia.def(py::init(), + py::arg("Ixx")=0, py::arg("Iyy")=0, py::arg("Izz")=0, + py::arg("Ixy")=0, py::arg("Ixz")=0, py::arg("Iyz")=0); + rotational_inertia.def_static("Zero", &RotationalInertia::Zero); + rotational_inertia.def("__getitem__", [](const RotationalInertia &inertia, int i) + { + if (i < 0 || i > 8) + throw py::index_error("RotationalInertia index out of range"); + + return inertia.data[i]; + }, py::arg("index")); + rotational_inertia.def("__setitem__", [](RotationalInertia &inertia, int i, double value) + { + if (i < 0 || i > 8) + throw py::index_error("RotationalInertia index out of range"); + + inertia.data[i] = value; + }, py::arg("index"), py::arg("value")); + rotational_inertia.def(double() * py::self); + rotational_inertia.def(py::self + py::self); + rotational_inertia.def(py::self * Vector()); + + + // -------------------- + // RigidBodyInertia + // -------------------- + py::class_ rigid_body_inertia(m, "RigidBodyInertia"); + rigid_body_inertia.def(py::init(), + py::arg("m")=0, py::arg_v("oc", Vector::Zero(), "Vector.Zero"), + py::arg_v("Ic", RotationalInertia::Zero(), "RigidBodyInertia.Zero")); + rigid_body_inertia.def_static("Zero", &RigidBodyInertia::Zero); + rigid_body_inertia.def("RefPoint", &RigidBodyInertia::RefPoint, py::arg("p")); + rigid_body_inertia.def("getMass", &RigidBodyInertia::getMass); + rigid_body_inertia.def("getCOG", &RigidBodyInertia::getCOG); + rigid_body_inertia.def("getRotationalInertia", &RigidBodyInertia::getRotationalInertia); + rigid_body_inertia.def(double() * py::self); + rigid_body_inertia.def(py::self + py::self); + rigid_body_inertia.def(py::self * Twist()); + rigid_body_inertia.def(Frame() * py::self); + rigid_body_inertia.def(Rotation() * py::self); + + + // -------------------- + // Segment + // -------------------- + py::class_ segment(m, "Segment"); + segment.def(py::init(), + py::arg("name"), py::arg_v("joint", Joint(), "Joint"), py::arg_v("f_tip", Frame::Identity(), "Frame.Identity"), + py::arg_v("I", RigidBodyInertia::Zero(), "RigidBodyInertia.Zero")); + segment.def(py::init(), + py::arg_v("joint", Joint(), "Joint"), py::arg_v("f_tip", Frame::Identity(), "Frame.Identity"), + py::arg_v("I", RigidBodyInertia::Zero(), "RigidBodyInertia.Zero")); + segment.def(py::init()); + segment.def("getFrameToTip", &Segment::getFrameToTip); + segment.def("pose", &Segment::pose, py::arg("q")); + segment.def("twist", &Segment::twist, py::arg("q"), py::arg("q_dot")); + segment.def("getName", &Segment::getName); + segment.def("getJoint", &Segment::getJoint); + segment.def("getInertia", &Segment::getInertia); + segment.def("setInertia", &Segment::setInertia, py::arg("I_in")); + + + // -------------------- + // Chain + // -------------------- + py::class_ chain(m, "Chain"); + chain.def(py::init<>()); + chain.def(py::init()); + chain.def("addSegment", &Chain::addSegment, py::arg("segment")); + chain.def("addChain", &Chain::addChain, py::arg("chain")); + chain.def("getNrOfJoints", &Chain::getNrOfJoints); + chain.def("getNrOfSegments", &Chain::getNrOfSegments); + chain.def("getSegment", (Segment& (Chain::*)(unsigned int)) &Chain::getSegment, py::arg("index")); + chain.def("getSegment", (const Segment& (Chain::*)(unsigned int) const) &Chain::getSegment, py::arg("index")); + chain.def("__repr__", [](const Chain &c) + { + std::ostringstream oss; + oss << c; + return oss.str(); + }); + + + // -------------------- + // Tree + // -------------------- + py::class_ tree(m, "Tree"); + tree.def(py::init(), py::arg("root_name")="root"); + tree.def("addSegment", &Tree::addSegment, py::arg("segment"), py::arg("hook_name")); + tree.def("addChain", &Tree::addChain, py::arg("chain"), py::arg("hook_name")); + tree.def("addTree", &Tree::addTree, py::arg("tree"), py::arg("hook_name")); + tree.def("getNrOfJoints", &Tree::getNrOfJoints); + tree.def("getNrOfSegments", &Tree::getNrOfSegments); + tree.def("getChain", [](const Tree &tree, const std::string& chain_root, const std::string& chain_tip) + { + Chain* chain = new Chain(); + tree.getChain(chain_root, chain_tip, *chain); + return chain; + }, py::arg("chain_root"), py::arg("chain_tip")); + tree.def("__repr__", [](const Tree &t) + { + std::ostringstream oss; + oss << t; + return oss.str(); + }); + + + // -------------------- + // Jacobian + // -------------------- + py::class_ jacobian(m, "Jacobian"); + jacobian.def(py::init<>()); + jacobian.def(py::init(), py::arg("nr_columns")); + jacobian.def(py::init()); + jacobian.def("rows", &Jacobian::rows); + jacobian.def("columns", &Jacobian::columns); + jacobian.def("resize", &Jacobian::resize, py::arg("nr_columns")); + jacobian.def("getColumn", &Jacobian::getColumn, py::arg("index")); + jacobian.def("setColumn", &Jacobian::setColumn, py::arg("index"), py::arg("t")); + jacobian.def("changeRefPoint", &Jacobian::changeRefPoint, py::arg("base")); + jacobian.def("changeBase", &Jacobian::changeBase, py::arg("rot")); + jacobian.def("changeRefFrame", &Jacobian::changeRefFrame, py::arg("frame")); + jacobian.def("__getitem__", [](const Jacobian &jac, std::tuple idx) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 5 || j < 0 || (unsigned int)j >= jac.columns()) + throw py::index_error("Jacobian index out of range"); + return jac((unsigned int)i, (unsigned int)j); + }, py::arg("index")); + jacobian.def("__setitem__", [](Jacobian &jac, std::tuple idx, double value) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 5 || j < 0 || (unsigned int)j >= jac.columns()) + throw py::index_error("Jacobian index out of range"); + + jac((unsigned int)i, (unsigned int)j) = value; + }, py::arg("index"), py::arg("value")); + jacobian.def("__repr__", [](const Jacobian &jac) + { + std::ostringstream oss; + oss << jac; + return oss.str(); + }); + + m.def("SetToZero", (void (*)(Jacobian&)) &KDL::SetToZero, py::arg("jac")); + m.def("changeRefPoint", (void (*)(const Jacobian&, const Vector&, Jacobian&)) &KDL::changeRefPoint, py::arg("src"), py::arg("base"), py::arg("dest")); + m.def("changeBase", (void (*)(const Jacobian&, const Rotation&, Jacobian&)) &KDL::changeBase, py::arg("src"), py::arg("rot"), py::arg("dest")); + m.def("SetToZero", (void (*)(const Jacobian&, const Frame&, Jacobian&)) &KDL::changeRefFrame, py::arg("src"), py::arg("frame"), py::arg("dest")); + + + // -------------------- + // JntArray + // -------------------- + py::class_ jnt_array(m, "JntArray"); + jnt_array.def(py::init<>()); + jnt_array.def(py::init(), py::arg("size")); + jnt_array.def(py::init()); + jnt_array.def("rows", &JntArray::rows); + jnt_array.def("columns", &JntArray::columns); + jnt_array.def("resize", &JntArray::resize, py::arg("size")); + jnt_array.def("__getitem__", [](const JntArray &ja, int i) + { + if (i < 0 || (unsigned int)i >= ja.rows()) + throw py::index_error("JntArray index out of range"); + + return ja(i); + }, py::arg("index")); + jnt_array.def("__setitem__", [](JntArray &ja, int i, double value) + { + if (i < 0 || (unsigned int)i >= ja.rows()) + throw py::index_error("JntArray index out of range"); + + ja(i) = value; + }, py::arg("index"), py::arg("value")); + jnt_array.def("__repr__", [](const JntArray &ja) + { + std::ostringstream oss; + oss << ja; + return oss.str(); + }); + jnt_array.def(py::self == py::self); + + m.def("Add", (void (*)(const JntArray&, const JntArray&, JntArray&)) &KDL::Add, py::arg("src1"), py::arg("src2"), py::arg("dest")); + m.def("Subtract", (void (*)(const JntArray&, const JntArray&, JntArray&)) &KDL::Subtract, py::arg("src1"), py::arg("src2"), py::arg("dest")); + m.def("Multiply", (void (*)(const JntArray&, const double&, JntArray&)) &KDL::Multiply, py::arg("src"), py::arg("factor"), py::arg("dest")); + m.def("Divide", (void (*)(const JntArray&, const double&, JntArray&)) &KDL::Divide, py::arg("src"), py::arg("factor"), py::arg("dest")); + m.def("MultiplyJacobian", (void (*)(const Jacobian&, const JntArray&, Twist&)) &KDL::MultiplyJacobian, py::arg("jac"), py::arg("src"), py::arg("dest")); + m.def("SetToZero", (void (*)(JntArray&)) &KDL::SetToZero, py::arg("jnt_array")); + m.def("Equal", (bool (*)(const JntArray&, const JntArray&, double)) &KDL::Equal, + py::arg("src1"), py::arg("src2"), py::arg("eps")=epsilon); + + + // -------------------- + // JntArrayVel + // -------------------- + py::class_ jnt_array_vel(m, "JntArrayVel"); + jnt_array_vel.def_readwrite("q", &JntArrayVel::q); + jnt_array_vel.def_readwrite("qdot", &JntArrayVel::qdot); + jnt_array_vel.def(py::init(), py::arg("size")); + jnt_array_vel.def(py::init(), py::arg("q"), py::arg("q_dot")); + jnt_array_vel.def(py::init()); + jnt_array_vel.def("resize", &JntArrayVel::resize, py::arg("size")); + jnt_array_vel.def("value", &JntArrayVel::value); + jnt_array_vel.def("deriv", &JntArrayVel::deriv); + + m.def("Add", (void (*)(const JntArrayVel&, const JntArrayVel&, JntArrayVel&)) &KDL::Add, py::arg("src1"), py::arg("src2"), py::arg("dest")); + m.def("Add", (void (*)(const JntArrayVel&, const JntArray&, JntArrayVel&)) &KDL::Add, py::arg("src1"), py::arg("src2"), py::arg("dest")); + m.def("Subtract", (void (*)(const JntArrayVel&, const JntArrayVel&, JntArrayVel&)) &KDL::Subtract, py::arg("src1"), py::arg("src2"), py::arg("dest")); + m.def("Subtract", (void (*)(const JntArrayVel&, const JntArray&, JntArrayVel&)) &KDL::Subtract, py::arg("src1"), py::arg("src2"), py::arg("dest")); + m.def("Multiply", (void (*)(const JntArrayVel&, const double&, JntArrayVel&)) &KDL::Multiply, py::arg("src"), py::arg("factor"), py::arg("dest")); + m.def("Multiply", (void (*)(const JntArrayVel&, const doubleVel&, JntArrayVel&)) &KDL::Multiply, py::arg("src"), py::arg("factor"), py::arg("dest")); + m.def("Divide", (void (*)(const JntArrayVel&, const double&, JntArrayVel&)) &KDL::Divide, py::arg("src"), py::arg("factor"), py::arg("dest")); + m.def("Divide", (void (*)(const JntArrayVel&, const doubleVel&, JntArrayVel&)) &KDL::Divide, py::arg("src"), py::arg("factor"), py::arg("dest")); + m.def("SetToZero", (void (*)(JntArrayVel&)) &KDL::SetToZero, py::arg("jnt_array_vel")); + m.def("Equal", (bool (*)(const JntArrayVel&, const JntArrayVel&, double)) &KDL::Equal, + py::arg("src1"), py::arg("src2"), py::arg("eps")=epsilon); + + + // -------------------- + // SolverI + // -------------------- + py::class_ solver_i(m, "SolverI"); + solver_i.def("getError", &SolverI::getError); + solver_i.def("strError", &SolverI::strError, py::arg("error")); + solver_i.def("updateInternalDataStructures", &SolverI::updateInternalDataStructures); + + + // -------------------- + // ChainFkSolverPos + // -------------------- + py::class_ chain_fk_solver_pos(m, "ChainFkSolverPos"); + chain_fk_solver_pos.def("JntToCart", (int (ChainFkSolverPos::*)(const JntArray&, Frame&, int)) &ChainFkSolverPos::JntToCart, + py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1); +// Argument by reference doesn't work for container types +// chain_fk_solver_pos.def("JntToCart", (int (ChainFkSolverPos::*)(const JntArray&, std::vector&, int)) &ChainFkSolverPos::JntToCart, +// py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1); + + + // -------------------- + // ChainFkSolverVel + // -------------------- + py::class_ chain_fk_solver_vel(m, "ChainFkSolverVel"); + chain_fk_solver_vel.def("JntToCart", (int (ChainFkSolverVel::*)(const JntArrayVel&, FrameVel&, int)) &ChainFkSolverVel::JntToCart, + py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1); +// Argument by reference doesn't work for container types +// chain_fk_solver_vel.def("JntToCart", (int (ChainFkSolverVel::*)(const JntArrayVel&, std::vector&, int)) &ChainFkSolverVel::JntToCart, +// py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1); + + + // ------------------------------ + // ChainFkSolverPos_recursive + // ------------------------------ + py::class_ chain_fk_solver_pos_recursive(m, "ChainFkSolverPos_recursive"); + chain_fk_solver_pos_recursive.def(py::init(), py::arg("chain")); + + + // ------------------------------ + // ChainFkSolverVel_recursive + // ------------------------------ + py::class_ chain_fk_solver_vel_recursive(m, "ChainFkSolverVel_recursive"); + chain_fk_solver_vel_recursive.def(py::init(), py::arg("chain")); + + + // -------------------- + // ChainIkSolverPos + // -------------------- + py::class_ chain_ik_solver_pos(m, "ChainIkSolverPos"); + chain_ik_solver_pos.def("CartToJnt", (int (ChainIkSolverPos::*)(const JntArray&, const Frame&, JntArray&)) &ChainIkSolverPos::CartToJnt, + py::arg("q_init"), py::arg("p_in"), py::arg("q_out")); + + + // -------------------- + // ChainIkSolverVel + // -------------------- + py::class_ chain_ik_solver_vel(m, "ChainIkSolverVel"); + chain_ik_solver_vel.def("CartToJnt", (int (ChainIkSolverVel::*)(const JntArray&, const Twist&, JntArray&)) &ChainIkSolverVel::CartToJnt, + py::arg("q_in"), py::arg("v_in"), py::arg("q_dot_out")); +// Not yet implemented in orocos_kdl +// chain_ik_solver_vel.def("CartToJnt", (int (ChainIkSolverVel::*)(const JntArray&, const FrameVel&, JntArrayVel&)) &ChainIkSolverVel::CartToJnt, +// py::arg("q_init"), py::arg("v_in"), py::arg("q_out")); + + + // ---------------------- + // ChainIkSolverPos_NR + // ---------------------- + py::class_ chain_ik_solver_pos_NR(m, "ChainIkSolverPos_NR"); + chain_ik_solver_pos_NR.def(py::init(), + py::arg("chain"), py::arg("fksolver"), py::arg("iksolver"), + py::arg("maxiter")=100, py::arg("eps")=epsilon); + + + // --------------------------- + // ChainIkSolverPos_NR_JL + // --------------------------- + py::class_ chain_ik_solver_pos_NR_JL(m, "ChainIkSolverPos_NR_JL"); + chain_ik_solver_pos_NR_JL.def(py::init(), + py::arg("chain"), py::arg("q_min"), py::arg("q_max"), py::arg("fksolver"), + py::arg("iksolver"), py::arg("maxiter")=100, py::arg("eps")=epsilon); + + + // ------------------------- + // ChainIkSolverVel_pinv + // ------------------------- + py::class_ chain_ik_solver_vel_pinv(m, "ChainIkSolverVel_pinv"); + chain_ik_solver_vel_pinv.def(py::init(), + py::arg("chain"), py::arg("eps")=0.00001, py::arg("maxiter")=150); + + + // ------------------------- + // ChainIkSolverVel_wdls + // ------------------------- + py::class_ chain_ik_solver_vel_wdls(m, "ChainIkSolverVel_wdls"); + chain_ik_solver_vel_wdls.def(py::init(), + py::arg("chain"), py::arg("eps")=0.00001, py::arg("maxiter")=150); + chain_ik_solver_vel_wdls.def("setWeightTS", &ChainIkSolverVel_wdls::setWeightTS, py::arg("matrix")); + chain_ik_solver_vel_wdls.def("setWeightJS", &ChainIkSolverVel_wdls::setWeightJS, py::arg("matrix")); + chain_ik_solver_vel_wdls.def("setLambda", &ChainIkSolverVel_wdls::setLambda, py::arg("lambda")); + chain_ik_solver_vel_wdls.def("setEps", &ChainIkSolverVel_wdls::setEps, py::arg("eps")); + chain_ik_solver_vel_wdls.def("setMaxIter", &ChainIkSolverVel_wdls::setMaxIter, py::arg("max_iter")); + chain_ik_solver_vel_wdls.def("getNrZeroSigmas", &ChainIkSolverVel_wdls::getNrZeroSigmas); + chain_ik_solver_vel_wdls.def("getSigmaMin", &ChainIkSolverVel_wdls::getSigmaMin); + chain_ik_solver_vel_wdls.def("getSigma", &ChainIkSolverVel_wdls::getSigma, py::arg("sigma_out")); + chain_ik_solver_vel_wdls.def("getEps", &ChainIkSolverVel_wdls::getEps); + chain_ik_solver_vel_wdls.def("getLambda", &ChainIkSolverVel_wdls::getLambda); + chain_ik_solver_vel_wdls.def("getLambdaScaled", &ChainIkSolverVel_wdls::getLambdaScaled); + chain_ik_solver_vel_wdls.def("getSVDResult", &ChainIkSolverVel_wdls::getSVDResult); + + + // ------------------------- + // ChainIkSolverPos_LMA + // ------------------------- + py::class_ chain_ik_solver_pos_LMA(m, "ChainIkSolverPos_LMA"); + chain_ik_solver_pos_LMA.def(py::init&, double, int, double>(), + py::arg("chain"), py::arg("L"), py::arg("eps")=1e-5, py::arg("maxiter")=500, + py::arg("eps_joints")=1e-15); + chain_ik_solver_pos_LMA.def(py::init(), + py::arg("chain"), py::arg("eps")=1e-5, py::arg("maxiter")=500, + py::arg("eps_joints")=1e-15); + + + // ---------------------------- + // ChainIkSolverVel_pinv_nso + // ---------------------------- + py::class_ chain_ik_solver_vel_pinv_nso(m, "ChainIkSolverVel_pinv_nso"); + chain_ik_solver_vel_pinv_nso.def(py::init(), + py::arg("chain"), py::arg("eps")=0.00001, py::arg("maxiter")=150, py::arg("alpha")=0.25); + chain_ik_solver_vel_pinv_nso.def("setWeights", &ChainIkSolverVel_pinv_nso::setWeights, py::arg("weights")); + chain_ik_solver_vel_pinv_nso.def("setOptPos", &ChainIkSolverVel_pinv_nso::setOptPos, py::arg("opt_pos")); + chain_ik_solver_vel_pinv_nso.def("setAlpha", &ChainIkSolverVel_pinv_nso::setAlpha, py::arg("alpha")); + chain_ik_solver_vel_pinv_nso.def("getWeights", &ChainIkSolverVel_pinv_nso::getWeights); + chain_ik_solver_vel_pinv_nso.def("getOptPos", &ChainIkSolverVel_pinv_nso::getOptPos); + chain_ik_solver_vel_pinv_nso.def("getAlpha", &ChainIkSolverVel_pinv_nso::getAlpha); + + + // ------------------------------- + // ChainIkSolverVel_pinv_givens + // ------------------------------- + py::class_ chain_ik_solver_vel_pinv_givens(m, "ChainIkSolverVel_pinv_givens"); + chain_ik_solver_vel_pinv_givens.def(py::init(), py::arg("chain")); + + + // ------------------------------ + // ChainJntToJacSolver + // ------------------------------ + py::class_ chain_jnt_to_jac_solver(m, "ChainJntToJacSolver"); + chain_jnt_to_jac_solver.def(py::init(), py::arg("chain")); + chain_jnt_to_jac_solver.def("JntToJac", &ChainJntToJacSolver::JntToJac, + py::arg("q_in"), py::arg("jac"), py::arg("seg_nr")=-1); + chain_jnt_to_jac_solver.def("setLockedJoints", &ChainJntToJacSolver::setLockedJoints, py::arg("locked_joints")); + + + // ------------------------------ + // ChainJntToJacDotSolver + // ------------------------------ + py::class_ chain_jnt_to_jac_dot_solver(m, "ChainJntToJacDotSolver"); + chain_jnt_to_jac_dot_solver.def(py::init(), py::arg("chain")); + chain_jnt_to_jac_dot_solver.def("JntToJacDot", (int (ChainJntToJacDotSolver::*)(const JntArrayVel&, Jacobian&, int)) &ChainJntToJacDotSolver::JntToJacDot, + py::arg("q_in"), py::arg("jdot"), py::arg("seg_nr")=-1); + chain_jnt_to_jac_dot_solver.def("JntToJacDot", (int (ChainJntToJacDotSolver::*)(const JntArrayVel&, Twist&, int)) &ChainJntToJacDotSolver::JntToJacDot, + py::arg("q_in"), py::arg("jac_dot_q_dot"), py::arg("seg_nr")=-1); + chain_jnt_to_jac_dot_solver.def("setLockedJoints", &ChainJntToJacDotSolver::setLockedJoints, + py::arg("locked_joints")); + + chain_jnt_to_jac_dot_solver.def_readonly_static("E_JAC_DOT_FAILED", &ChainJntToJacDotSolver::E_JAC_DOT_FAILED); + chain_jnt_to_jac_dot_solver.def_readonly_static("E_JACSOLVER_FAILED", &ChainJntToJacDotSolver::E_JACSOLVER_FAILED); + chain_jnt_to_jac_dot_solver.def_readonly_static("E_FKSOLVERPOS_FAILED", &ChainJntToJacDotSolver::E_FKSOLVERPOS_FAILED); + + chain_jnt_to_jac_dot_solver.def_readonly_static("HYBRID", &ChainJntToJacDotSolver::HYBRID); + chain_jnt_to_jac_dot_solver.def_readonly_static("BODYFIXED", &ChainJntToJacDotSolver::BODYFIXED); + chain_jnt_to_jac_dot_solver.def_readonly_static("INERTIAL", &ChainJntToJacDotSolver::INERTIAL); + + chain_jnt_to_jac_dot_solver.def("setHybridRepresentation", &ChainJntToJacDotSolver::setHybridRepresentation); + chain_jnt_to_jac_dot_solver.def("setBodyFixedRepresentation", &ChainJntToJacDotSolver::setBodyFixedRepresentation); + chain_jnt_to_jac_dot_solver.def("setInertialRepresentation", &ChainJntToJacDotSolver::setInertialRepresentation); + chain_jnt_to_jac_dot_solver.def("setRepresentation", &ChainJntToJacDotSolver::setRepresentation, + py::arg("representation")); + + + // ------------------------------ + // ChainIdSolver + // ------------------------------ + py::class_ chain_id_solver(m, "ChainIdSolver"); + chain_id_solver.def("CartToJnt", &ChainIdSolver::CartToJnt, py::arg("q"), py::arg("q_dot"), py::arg("q_dot_dot"), py::arg("f_ext"), py::arg("torques")); + + + // ------------------------------ + // ChainIdSolver_RNE + // ------------------------------ + py::class_ chain_id_solver_RNE(m, "ChainIdSolver_RNE"); + chain_id_solver_RNE.def(py::init(), py::arg("chain"), py::arg("grav")); +} diff --git a/python_orocos_kdl/PyKDL/kinfam.sip b/python_orocos_kdl/PyKDL/kinfam.sip deleted file mode 100644 index f1507e41f..000000000 --- a/python_orocos_kdl/PyKDL/kinfam.sip +++ /dev/null @@ -1,702 +0,0 @@ -//Copyright (C) 2007 Ruben Smits -// -//Version: 1.0 -//Author: Ruben Smits -//Maintainer: Ruben Smits -//URL: http://www.orocos.org/kdl -// -//This library is free software; you can redistribute it and/or -//modify it under the terms of the GNU Lesser General Public -//License as published by the Free Software Foundation; either -//version 2.1 of the License, or (at your option) any later version. -// -//This library is distributed in the hope that it will be useful, -//but WITHOUT ANY WARRANTY; without even the implied warranty of -//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -//Lesser General Public License for more details. -// -//You should have received a copy of the GNU Lesser General Public -//License along with this library; if not, write to the Free Software -//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA - - -template -%MappedType std::vector -{ -%TypeHeaderCode -#include -%End - -%ConvertFromTypeCode - PyObject *l = PyList_New(sipCpp -> size()); - - // Create the Python list of the correct length. - if (!l) - return NULL; - - // Go through each element in the C++ instance and convert it to a - // wrapped P2d. - for (int i = 0; i < (int)sipCpp->size(); ++i) { - TYPE *cpp = new TYPE(sipCpp->at(i)); - PyObject *pobj = sipConvertFromInstance(cpp, sipClass_TYPE, sipTransferObj); - - // Get the Python wrapper for the Type instance, creating a new - // one if necessary, and handle any ownership transfer. - if (!pobj) { - // There was an error so garbage collect the Python list. - Py_DECREF(l); - return NULL; - } - - // Add the wrapper to the list. - PyList_SET_ITEM(l, i, pobj); - } - - // Return the Python list. - return l; -%End - -%ConvertToTypeCode - // Check if type is compatible - if (!sipIsErr) { - // Must be any iterable - PyObject *i = PyObject_GetIter(sipPy); - bool iterable = (i != NULL); - Py_XDECREF(i); - return iterable; - } - - // Iterate over the object - PyObject *iterator = PyObject_GetIter(sipPy); - PyObject *item; - - std::vector *V = new std::vector(); - - while ((item = PyIter_Next(iterator))) - { - if (!sipCanConvertToInstance(item, sipClass_TYPE, SIP_NOT_NONE)) { - PyErr_Format(PyExc_TypeError, "object in iterable cannot be converted to TYPE"); - *sipIsErr = 1; - break; - } - - int state; - TYPE* p = reinterpret_cast( - sipConvertToInstance(item, sipClass_TYPE, 0, SIP_NOT_NONE, &state, sipIsErr)); - - if (!*sipIsErr) - V->push_back(*p); - - sipReleaseInstance(p, sipClass_TYPE, state); - Py_DECREF(item); - } - - Py_DECREF(iterator); - - if (*sipIsErr) { - delete V; - return 0; - } - - *sipCppPtr = V; - return sipGetState(sipTransferObj); -%End -}; - - - -class Joint{ - -%TypeHeaderCode -#include -#include -using namespace KDL; -%End - - -public: - enum JointType {RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,None}; - Joint(std::string name, JointType type=None,double scale=1,double offset=0, - double inertia=0,double damping=0,double stiffness=0); - Joint(JointType type=None,double scale=1,double offset=0, - double inertia=0,const double damping=0,double stiffness=0); - Joint(std::string name, Vector origin, Vector axis, JointType type, double scale=1, double offset=0, - double inertia=0, double damping=0, double stiffness=0); - Joint(Vector origin, Vector axis, JointType type, double scale=1, double offset=0, - double inertia=0, double damping=0, double stiffness=0); - - Joint(const Joint& in); - - Frame pose(const double& q)const /Factory/ ; - Twist twist(const double& qdot)const /Factory/ ; - Vector JointAxis() const /Factory/; - Vector JointOrigin() const /Factory/; - std::string getName()const; - - JointType getType() const; - std::string getTypeName() const; - const std::string* __repr__(); - %MethodCode - std::ostringstream oss; - oss<<(*sipCpp); - std::string s(oss.str()); - sipRes=&s; - %End -}; - -class RotationalInertia -{ -%TypeHeaderCode -#include -#include -using namespace KDL; -%End -public: - RotationalInertia(double Ixx=0,double Iyy=0,double Izz=0,double Ixy=0,double Ixz=0,double Iyz=0); - - static RotationalInertia Zero()/Factory/; - - double __getitem__(int index); -%MethodCode - if (a0 < 0 || a0 >= 9) { - PyErr_SetString(PyExc_IndexError, "RotationalInertia index out of range"); - return 0; - } - sipRes=(*sipCpp).data[a0]; -%End - - void __setitem__(int i, double value); -%MethodCode - if (a0 < 0 || a0 >= 9) { - PyErr_SetString(PyExc_IndexError, "RotationalInertia index out of range"); - return 0; - } - (*sipCpp).data[a0]=a1; -%End - -}; -Vector operator*(RotationalInertia& Ia, Vector omega) const /Factory/; -RotationalInertia operator*(double a, const RotationalInertia& I)/Factory/; -RotationalInertia operator+(const RotationalInertia& Ia, const RotationalInertia& Ib)/Factory/; - - -class RigidBodyInertia -{ -%TypeHeaderCode -#include -#include -using namespace KDL; -%End -public: - RigidBodyInertia(double m=0, const Vector& oc=Vector::Zero(), const RotationalInertia& Ic=RotationalInertia::Zero()); - static RigidBodyInertia Zero() /Factory/; - RigidBodyInertia RefPoint(const Vector& p) /Factory/; - double getMass()const /Factory/; - Vector getCOG() const /Factory/; - RotationalInertia getRotationalInertia() const /Factory/; -}; -RigidBodyInertia operator*(double a,const RigidBodyInertia& I) /Factory/; -RigidBodyInertia operator+(const RigidBodyInertia& Ia,const RigidBodyInertia& Ib) /Factory/; -Wrench operator*(const RigidBodyInertia& I,const Twist& t) /Factory/; -RigidBodyInertia operator*(const Frame& T,const RigidBodyInertia& I) /Factory/; -RigidBodyInertia operator*(const Rotation& R,const RigidBodyInertia& I) /Factory/; - -class Segment -{ - -%TypeHeaderCode -#include -#include -using namespace KDL; -%End -public: - Segment(const std::string& name, const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); - Segment(const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); - Segment(const Segment& in); - - const std::string* __repr__(); -%MethodCode - std::stringstream ss; - ss<<(*sipCpp); - std::string s(ss.str()); - sipRes=&s; -%End - - const Frame& getFrameToTip()const /Factory/; - Frame pose(const double& q)const /Factory/ ; - Twist twist(const double& q,const double& qdot)const /Factory/ ; - const std::string& getName()const /Factory/; - const Joint& getJoint()const /Factory/; - const RigidBodyInertia& getInertia()const /Factory/; - void setInertia(const RigidBodyInertia& Iin); -}; - -class Chain -{ - -%TypeHeaderCode -#include -using namespace KDL; -%End - -public: - Chain(); - Chain(const Chain& in); - - void addSegment(const Segment& segment); - void addChain(const Chain& chain); - - unsigned int getNrOfJoints()const; - unsigned int getNrOfSegments()const; - - const Segment& getSegment(unsigned int nr)const /Factory/; - -}; - -class Tree { -%TypeHeaderCode -#include -using namespace KDL; -%End -public: - Tree(const std::string& root_name="root"); - bool addSegment(const Segment& segment, const std::string& hook_name); - unsigned int getNrOfJoints()const; - unsigned int getNrOfSegments()const; - Chain* getChain(const std::string& chain_root, const std::string& chain_tip)const; -%MethodCode - Chain* chain = new Chain(); - sipCpp->getChain(*a0, *a1, *chain); - sipRes = chain; -%End -}; - -class JntArray{ - -%TypeHeaderCode -#include -using namespace KDL; -%End - -public: - JntArray(unsigned int size); - JntArray(const JntArray& arg); - unsigned int rows()const; - unsigned int columns()const; - void resize(unsigned int newSize); - - double __getitem__ (int index); -%MethodCode - if (a0 < 0 || a0 >= (int)sipCpp->rows()) { - PyErr_SetString(PyExc_IndexError, "JntArray index out of range"); - return 0; - } - sipRes=(*sipCpp)(a0); -%End - - void __setitem__(int index, double value); -%MethodCode - if (a0 < 0 || a0 >= (int)sipCpp->rows()) { - PyErr_SetString(PyExc_IndexError, "JntArray index out of range"); - return 0; - } - (*sipCpp)(a0)=a1; -%End - - const std::string* __repr__(); -%MethodCode - std::stringstream ss; - ss<data; - std::string s(ss.str()); - sipRes=&s; -%End -}; - -void Add(const JntArray& src1,const JntArray& src2,JntArray& dest); -void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest); -void Multiply(const JntArray& src,const double& factor,JntArray& dest); -void Divide(const JntArray& src,const double& factor,JntArray& dest); -void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest); -void SetToZero(JntArray& array); -bool Equal(const JntArray& src1,const JntArray& src2,double eps=epsilon); -bool operator==(const JntArray& src1,const JntArray& src2); -//bool operator!=(const JntArray& src1,const JntArray& src2); - -class JntArrayVel -{ -%TypeHeaderCode -#include -using namespace KDL; -%End - -public: - JntArray q; - JntArray qdot; - JntArrayVel(unsigned int size); - JntArrayVel(const JntArray& q,const JntArray& qdot); - JntArrayVel(const JntArray& q); - void resize(unsigned int newSize); - - JntArray value()const /Factory/; - JntArray deriv()const /Factory/; -}; - -void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest); -void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest); -void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest); -void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest); -void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest); -void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest); -void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest); -void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest); -void SetToZero(JntArrayVel& array); -bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps=epsilon); - -class Jacobian -{ -%TypeHeaderCode -#include -using namespace KDL; -%End -public: - Jacobian(unsigned int size); - Jacobian(const Jacobian& arg); - unsigned int rows()const; - unsigned int columns()const; - void resize(unsigned int newNrOfColumns); - - double __getitem__ (SIP_PYTUPLE); -%MethodCode - int i,j; - PyArg_ParseTuple(a0,"ii",&i,&j); - if (i < 0 || j < 0 || i > 5 || j >= (int)sipCpp->columns()) { - PyErr_SetString(PyExc_IndexError, "Jacobian index out of range"); - return 0; - } - sipRes=(*sipCpp)(i,j); -%End - - void __setitem__(SIP_PYTUPLE,double value); -%MethodCode - int i,j; - PyArg_ParseTuple(a0,"ii",&i,&j); - if (i < 0 || j < 0 || i > 5 || j >= (int)sipCpp->columns()) { - PyErr_SetString(PyExc_IndexError, "Jacobian index out of range"); - return 0; - } - (*sipCpp)(i,j)=a1; -%End - - const std::string* __repr__(); -%MethodCode - std::stringstream ss; - ss<data; - std::string s(ss.str()); - sipRes=&s; -%End - Twist getColumn(unsigned int i) const /Factory/; - void setColumn(unsigned int i,const Twist& t); - - void changeRefPoint(const Vector& base_AB); - void changeBase(const Rotation& rot); - void changeRefFrame(const Frame& frame); - -}; -void SetToZero(Jacobian& jac); - -void changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest); -void changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest); -void changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest); - -class SolverI -{ -%TypeHeaderCode -#include -using namespace KDL; -%End - virtual int getError() const; - virtual const char* strError(const int error) const; - virtual void updateInternalDataStructures() = 0; -}; - -class ChainFkSolverPos : SolverI -{ -%TypeHeaderCode -#include -using namespace KDL; -%End - virtual int JntToCart(const JntArray& q_in, Frame& p_out,int segmentNr=-1)=0; -}; - -class ChainFkSolverVel : SolverI -{ -%TypeHeaderCode -#include -using namespace KDL; -%End - virtual int JntToCart(const JntArrayVel& q_in, FrameVel& p_out,int - segmentNr=-1)=0; -}; - -class ChainFkSolverPos_recursive : ChainFkSolverPos -{ -%TypeHeaderCode -#include -using namespace KDL; -%End - -public: - ChainFkSolverPos_recursive(const Chain& chain); - virtual int JntToCart(const JntArray& q_in, Frame& p_out,int segmentNr=-1); - virtual void updateInternalDataStructures(); -}; - -class ChainFkSolverVel_recursive : ChainFkSolverVel -{ -%TypeHeaderCode -#include -using namespace KDL; -%End -public: - ChainFkSolverVel_recursive(const Chain& chain); - virtual int JntToCart(const JntArrayVel& q_in ,FrameVel& out,int - segmentNr=-1 ); - virtual void updateInternalDataStructures(); -}; - -class ChainIkSolverPos : SolverI { -%TypeHeaderCode -#include -using namespace KDL; -%End -public: - virtual int CartToJnt(const JntArray& q_init , const Frame& p_in, JntArray& q_out )=0; - virtual void updateInternalDataStructures()=0; -}; - -class ChainIkSolverVel : SolverI { -%TypeHeaderCode -#include -using namespace KDL; -%End -public: - virtual int CartToJnt(const JntArray& q_in , const Twist& v_in , JntArray& qdot_out )=0; - virtual int CartToJnt(const JntArray& q_init , const FrameVel& v_in , JntArrayVel& q_out )=0; - virtual void updateInternalDataStructures()=0; -}; - -class ChainIkSolverPos_NR : ChainIkSolverPos -{ -%TypeHeaderCode -#include -using namespace KDL; -%End -public: - ChainIkSolverPos_NR(const Chain& chain,ChainFkSolverPos& fksolver,ChainIkSolverVel& iksolver, - unsigned int maxiter=100,double eps=epsilon); - - virtual int CartToJnt(const JntArray& q_init , const Frame& p_in ,JntArray& q_out); - virtual void updateInternalDataStructures(); -}; - -class ChainIkSolverPos_NR_JL : ChainIkSolverPos -{ -%TypeHeaderCode -#include -using namespace KDL; -%End -public: - ChainIkSolverPos_NR_JL(const Chain& chain,const JntArray &q_min,const JntArray &q_max, - ChainFkSolverPos& fksolver,ChainIkSolverVel& iksolver, - unsigned int maxiter=100,double eps=epsilon); - - virtual int CartToJnt(const JntArray& q_init , const Frame& p_in ,JntArray& q_out); - virtual void updateInternalDataStructures(); -}; - -class ChainIkSolverVel_pinv : ChainIkSolverVel -{ -%TypeHeaderCode -#include -using namespace KDL; -%End -public: - ChainIkSolverVel_pinv(const Chain& chain,double eps=0.00001,int maxiter=150); - - virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); - virtual void updateInternalDataStructures(); -}; - -class ChainIkSolverVel_wdls : ChainIkSolverVel -{ -%TypeHeaderCode -#include -using namespace KDL; -%End -public: - ChainIkSolverVel_wdls(const Chain& chain,double eps=0.00001,int maxiter=150); - - virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); - virtual void updateInternalDataStructures(); - - void setWeightTS(SIP_PYLIST); -%MethodCode - //void setWeightTS(const Eigen::MatrixXd& Mx); - //Mx has to be a 6x6 Matrix - - Py_ssize_t numRows,numCols; - double c_item; - PyObject *list=a0; - numRows=PyList_Size(list); - PyObject *temp1; - temp1=PyList_GetItem(list,0); - numCols=PyList_Size(temp1); - if (numRows!=numCols) { - sipIsErr=1; //todo: raise exception - } - if (numRows!=6) { - sipIsErr=1; //todo: raise exception - } - Eigen::MatrixXd Mx; - Mx=Eigen::MatrixXd::Identity(numRows,numCols); - - for (Py_ssize_t r=0;rsetWeightTS(Mx); -%End - - void setWeightJS(SIP_PYLIST); -%MethodCode - //void setWeightJS(const Eigen::MatrixXd& Mx); - //Mx has to be a simetric positive definite Matrix - //unsigned int nOfJoints=sipCpp->chain.getNrOfJoints(); //To check that we are receiving valid data dimensions. This doesn't work, chain is a private member. todo: How can we check for this? - Py_ssize_t numRows,numCols; - double c_item; - PyObject *list=a0; - numRows=PyList_Size(list); - PyObject *temp1; - temp1=PyList_GetItem(list,0); - numCols=PyList_Size(temp1); - if (numRows!=numCols) { - sipIsErr=1; //todo: raise exception - } - Eigen::MatrixXd Mq; - Mq=Eigen::MatrixXd::Identity(numRows,numCols); - for (Py_ssize_t r=0;rsetWeightJS(Mq); -%End - - void setLambda(const double& lambda); - -}; - - -class ChainIkSolverPos_LMA : ChainIkSolverPos -{ -%TypeHeaderCode -#include -using namespace KDL; -%End -public: - ChainIkSolverPos_LMA(const Chain& chain, double eps=0.00001, int _maxiter=500, double _eps_joints=0.000000000000001); - - virtual int CartToJnt(const JntArray& q_init , const Frame& p_in ,JntArray& q_out); - virtual void updateInternalDataStructures(); -}; - - -class ChainIkSolverVel_pinv_nso : ChainIkSolverVel -{ -%TypeHeaderCode -#include -using namespace KDL; -%End -public: - ChainIkSolverVel_pinv_nso(const Chain& chain,double eps=0.00001,int maxiter=150, double alpha=0.25); - - virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); - virtual void updateInternalDataStructures(); - - virtual int setWeights(const JntArray &weights); - - virtual int setOptPos(const JntArray &opt_pos); - - virtual int setAlpha(const double alpha); - - const JntArray& getWeights()const /Factory/; - - const JntArray& getOptPos()const /Factory/; - - const double& getAlpha()const /Factory/; -}; - -class ChainIkSolverVel_pinv_givens : ChainIkSolverVel -{ -%TypeHeaderCode -#include -using namespace KDL; -%End -public: - ChainIkSolverVel_pinv_givens(const Chain& chain); - - virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); - virtual void updateInternalDataStructures(); -}; - -class ChainJntToJacSolver : SolverI -{ -%TypeHeaderCode -#include -using namespace KDL; -%End -public: - ChainJntToJacSolver(const Chain& chain); - int JntToJac(const JntArray& q_in,Jacobian& jac); - virtual void updateInternalDataStructures(); -}; - - -class ChainIdSolver : SolverI -{ -%TypeHeaderCode -#include -using namespace KDL; -%End - virtual int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const std::vector& f_ext,JntArray &torques)=0; -}; - -class ChainIdSolver_RNE : ChainIdSolver -{ -%TypeHeaderCode -#include -using namespace KDL; -%End - -public: - ChainIdSolver_RNE(const Chain& chain,Vector grav); - int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const std::vector& f_ext,JntArray &torques); - virtual void updateInternalDataStructures(); -}; diff --git a/python_orocos_kdl/PyKDL/std_string.sip b/python_orocos_kdl/PyKDL/std_string.sip deleted file mode 100644 index d91bda972..000000000 --- a/python_orocos_kdl/PyKDL/std_string.sip +++ /dev/null @@ -1,71 +0,0 @@ -//Copyright (C) 2005 Torsten Marek -// -//This library is free software; you can redistribute it and/or -//modify it under the terms of the GNU Lesser General Public -//License as published by the Free Software Foundation; either -//version 2.1 of the License, or (at your option) any later version. -// -//This library is distributed in the hope that it will be useful, -//but WITHOUT ANY WARRANTY; without even the implied warranty of -//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -//Lesser General Public License for more details. -// -//You should have received a copy of the GNU Lesser General Public -//License along with this library; if not, write to the Free Software -//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA - - -%MappedType std::string -{ -%TypeHeaderCode -#include -%End - -%ConvertFromTypeCode - // convert an std::string to a Python (unicode) string - PyObject* newstring; - newstring = PyUnicode_DecodeUTF8(sipCpp->c_str(), sipCpp->length(), NULL); - if(newstring == NULL) { - PyErr_Clear(); - newstring = PyUnicode_FromString(sipCpp->c_str()); - } - return newstring; -%End - -%ConvertToTypeCode - // Allow a Python string (or a unicode string) whenever a string is - // expected. - // If argument is a Unicode string, just decode it to UTF-8 - // If argument is a Python string, assume it's UTF-8 - if (sipIsErr == NULL) -#if PY_MAJOR_VERSION < 3 - return (PyString_Check(sipPy) || PyUnicode_Check(sipPy)); -#else - return PyUnicode_Check(sipPy); -#endif - if (sipPy == Py_None) { - *sipCppPtr = new std::string; - return 1; - } -#if PY_MAJOR_VERSION < 3 - if (PyUnicode_Check(sipPy)) { - PyObject* s = PyUnicode_AsUTF8String(sipPy); - *sipCppPtr = new std::string(PyString_AS_STRING(s)); - Py_DECREF(s); - return 1; - } - else if (PyString_Check(sipPy)) { - *sipCppPtr = new std::string(PyString_AS_STRING(sipPy)); - return 1; - } -#else - if (PyUnicode_Check(sipPy)) { - *sipCppPtr = new std::string(PyUnicode_AsUTF8(sipPy)); - return 1; - } -#endif - - return 0; -%End -}; - diff --git a/python_orocos_kdl/cmake/FindEigen3.cmake b/python_orocos_kdl/cmake/FindEigen3.cmake deleted file mode 100644 index 3a9dc56ce..000000000 --- a/python_orocos_kdl/cmake/FindEigen3.cmake +++ /dev/null @@ -1,6 +0,0 @@ -FIND_PATH(EIGEN3_INCLUDE_DIR Eigen/Core /usr/include /usr/include/eigen3) -IF ( EIGEN3_INCLUDE_DIR ) - MESSAGE(STATUS "-- Looking for Eigen3 - found") -ELSE ( EIGEN3_INCLUDE_DIR ) - MESSAGE(FATAL_ERROR "-- Looking for Eigen3 - not found") -ENDIF ( EIGEN3_INCLUDE_DIR ) diff --git a/python_orocos_kdl/cmake/FindSIP.cmake b/python_orocos_kdl/cmake/FindSIP.cmake deleted file mode 100644 index 100474ba0..000000000 --- a/python_orocos_kdl/cmake/FindSIP.cmake +++ /dev/null @@ -1,67 +0,0 @@ -# Find SIP -# ~~~~~~~~ -# -# SIP website: http://www.riverbankcomputing.co.uk/sip/index.php -# -# Find the installed version of SIP. FindSIP should be called after Python -# has been found. -# -# This file defines the following variables: -# -# SIP_VERSION - The version of SIP found expressed as a 6 digit hex number -# suitable for comparison as a string. -# -# SIP_VERSION_STR - The version of SIP found as a human readable string. -# -# SIP_EXECUTABLE - Path and filename of the SIP command line executable. -# -# SIP_INCLUDE_DIR - Directory holding the SIP C++ header file. -# -# SIP_DEFAULT_SIP_DIR - Default directory where .sip files should be installed -# into. - -# Copyright (c) 2007, Simon Edwards -# Redistribution and use is allowed according to the terms of the BSD license. -# For details see the accompanying COPYING-CMAKE-SCRIPTS file. - - - -IF(SIP_VERSION) - # Already in cache, be silent - SET(SIP_FOUND TRUE) -ELSE(SIP_VERSION) - - FIND_FILE(_find_sip_py FindSIP.py PATHS ${CMAKE_MODULE_PATH} NO_CMAKE_FIND_ROOT_PATH) - - EXECUTE_PROCESS(COMMAND ${PYTHON_EXECUTABLE} ${_find_sip_py} OUTPUT_VARIABLE sip_config) - IF(sip_config) - STRING(REGEX REPLACE "^sip_version:([^\n]+).*$" "\\1" SIP_VERSION ${sip_config}) - STRING(REGEX REPLACE ".*\nsip_version_str:([^\n]+).*$" "\\1" SIP_VERSION_STR ${sip_config}) - IF(NOT SIP_DEFAULT_SIP_DIR) - STRING(REGEX REPLACE ".*\ndefault_sip_dir:([^\n]+).*$" "\\1" SIP_DEFAULT_SIP_DIR ${sip_config}) - ENDIF(NOT SIP_DEFAULT_SIP_DIR) - - IF(CMAKE_CROSSCOMPILING) - FIND_PROGRAM(SIP_EXECUTABLE sip) - STRING(REGEX REPLACE ".*\nsip_inc_dir:([^\n]+).*$" "\\1" SIP_INCLUDE_DIR ${sip_config}) - LIST(GET CMAKE_FIND_ROOT_PATH 0 SIP_SYSROOT) - SET(SIP_INCLUDE_DIR "${SIP_SYSROOT}${SIP_INCLUDE_DIR}") - ELSE(CMAKE_CROSSCOMPILING) - STRING(REGEX REPLACE ".*\nsip_bin:([^\n]+).*$" "\\1" SIP_EXECUTABLE ${sip_config}) - STRING(REGEX REPLACE ".*\nsip_inc_dir:([^\n]+).*$" "\\1" SIP_INCLUDE_DIR ${sip_config}) - ENDIF(CMAKE_CROSSCOMPILING) - - SET(SIP_FOUND TRUE) - ENDIF(sip_config) - - IF(SIP_FOUND) - IF(NOT SIP_FIND_QUIETLY) - MESSAGE(STATUS "Found SIP version: ${SIP_VERSION_STR}") - ENDIF(NOT SIP_FIND_QUIETLY) - ELSE(SIP_FOUND) - IF(SIP_FIND_REQUIRED) - MESSAGE(FATAL_ERROR "Could not find SIP") - ENDIF(SIP_FIND_REQUIRED) - ENDIF(SIP_FOUND) - -ENDIF(SIP_VERSION) diff --git a/python_orocos_kdl/cmake/FindSIP.py b/python_orocos_kdl/cmake/FindSIP.py deleted file mode 100644 index ecb734f2c..000000000 --- a/python_orocos_kdl/cmake/FindSIP.py +++ /dev/null @@ -1,15 +0,0 @@ -# FindSIP.py -# -# Copyright (c) 2007, Simon Edwards -# Redistribution and use is allowed according to the terms of the BSD license. -# For details see the accompanying COPYING-CMAKE-SCRIPTS file. - -import sys -import sipconfig - -sipcfg = sipconfig.Configuration() -print("sip_version:%06.0x" % sipcfg.sip_version) -print("sip_version_str:%s" % sipcfg.sip_version_str) -print("sip_bin:%s" % sipcfg.sip_bin) -print("default_sip_dir:%s" % sipcfg.default_sip_dir) -print("sip_inc_dir:%s" % sipcfg.sip_inc_dir) diff --git a/python_orocos_kdl/cmake/SIPMacros.cmake b/python_orocos_kdl/cmake/SIPMacros.cmake deleted file mode 100644 index 4464a9267..000000000 --- a/python_orocos_kdl/cmake/SIPMacros.cmake +++ /dev/null @@ -1,124 +0,0 @@ -# Macros for SIP -# ~~~~~~~~~~~~~~ -# Copyright (c) 2007, Simon Edwards -# Redistribution and use is allowed according to the terms of the BSD license. -# For details see the accompanying COPYING-CMAKE-SCRIPTS file. -# -# SIP website: http://www.riverbankcomputing.co.uk/sip/index.php -# -# This file defines the following macros: -# -# ADD_SIP_PYTHON_MODULE (MODULE_NAME MODULE_SIP [library1, libaray2, ...]) -# Specifies a SIP file to be built into a Python module and installed. -# MODULE_NAME is the name of Python module including any path name. (e.g. -# os.sys, Foo.bar etc). MODULE_SIP the path and filename of the .sip file -# to process and compile. libraryN are libraries that the Python module, -# which is typically a shared library, should be linked to. The built -# module will also be install into Python's site-packages directory. -# -# The behaviour of the ADD_SIP_PYTHON_MODULE macro can be controlled by a -# number of variables: -# -# SIP_INCLUDES - List of directories which SIP will scan through when looking -# for included .sip files. (Corresponds to the -I option for SIP.) -# -# SIP_TAGS - List of tags to define when running SIP. (Corresponds to the -t -# option for SIP.) -# -# SIP_CONCAT_PARTS - An integer which defines the number of parts the C++ code -# of each module should be split into. Defaults to 8. (Corresponds to the -# -j option for SIP.) -# -# SIP_DISABLE_FEATURES - List of feature names which should be disabled -# running SIP. (Corresponds to the -x option for SIP.) -# -# SIP_EXTRA_OPTIONS - Extra command line options which should be passed on to -# SIP. - -SET(SIP_INCLUDES) -SET(SIP_TAGS) -SET(SIP_CONCAT_PARTS 8) -SET(SIP_DISABLE_FEATURES) -SET(SIP_EXTRA_OPTIONS) - -MACRO(ADD_SIP_PYTHON_MODULE MODULE_NAME MODULE_SIP) - - SET(EXTRA_LINK_LIBRARIES ${ARGN}) - - STRING(REPLACE "." "/" _x ${MODULE_NAME}) - GET_FILENAME_COMPONENT(_parent_module_path ${_x} PATH) - GET_FILENAME_COMPONENT(_child_module_name ${_x} NAME) - - GET_FILENAME_COMPONENT(_module_path ${MODULE_SIP} PATH) - - if(_module_path STREQUAL "") - set(CMAKE_CURRENT_SIP_OUTPUT_DIR "${CMAKE_CURRENT_BINARY_DIR}") - else(_module_path STREQUAL "") - set(CMAKE_CURRENT_SIP_OUTPUT_DIR "${CMAKE_CURRENT_BINARY_DIR}/${_module_path}") - endif(_module_path STREQUAL "") - - GET_FILENAME_COMPONENT(_abs_module_sip ${MODULE_SIP} ABSOLUTE) - - # We give this target a long logical target name. - # (This is to avoid having the library name clash with any already - # install library names. If that happens then cmake dependency - # tracking get confused.) - STRING(REPLACE "." "_" _logical_name ${MODULE_NAME}) - SET(_logical_name "python_module_${_logical_name}") - - FILE(MAKE_DIRECTORY ${CMAKE_CURRENT_SIP_OUTPUT_DIR}) # Output goes in this dir. - - SET(_sip_includes) - FOREACH (_inc ${SIP_INCLUDES}) - GET_FILENAME_COMPONENT(_abs_inc ${_inc} ABSOLUTE) - LIST(APPEND _sip_includes -I ${_abs_inc}) - ENDFOREACH (_inc ) - - SET(_sip_tags) - FOREACH (_tag ${SIP_TAGS}) - LIST(APPEND _sip_tags -t ${_tag}) - ENDFOREACH (_tag) - - SET(_sip_x) - FOREACH (_x ${SIP_DISABLE_FEATURES}) - LIST(APPEND _sip_x -x ${_x}) - ENDFOREACH (_x ${SIP_DISABLE_FEATURES}) - - SET(_message "-DMESSAGE=Generating CPP code for module ${MODULE_NAME}") - SET(_sip_output_files) - FOREACH(CONCAT_NUM RANGE 0 ${SIP_CONCAT_PARTS} ) - IF( ${CONCAT_NUM} LESS ${SIP_CONCAT_PARTS} ) - SET(_sip_output_files ${_sip_output_files} ${CMAKE_CURRENT_SIP_OUTPUT_DIR}/sip${_child_module_name}part${CONCAT_NUM}.cpp ) - ENDIF( ${CONCAT_NUM} LESS ${SIP_CONCAT_PARTS} ) - ENDFOREACH(CONCAT_NUM RANGE 0 ${SIP_CONCAT_PARTS} ) - - IF(NOT WIN32) - SET(TOUCH_COMMAND touch) - ELSE(NOT WIN32) - SET(TOUCH_COMMAND echo) - # instead of a touch command, give out the name and append to the files - # this is basically what the touch command does. - FOREACH(filename ${_sip_output_files}) - FILE(APPEND filename "") - ENDFOREACH(filename ${_sip_output_files}) - ENDIF(NOT WIN32) - ADD_CUSTOM_COMMAND( - OUTPUT ${_sip_output_files} - COMMAND ${CMAKE_COMMAND} -E echo ${message} - COMMAND ${TOUCH_COMMAND} ${_sip_output_files} - COMMAND ${SIP_EXECUTABLE} ${_sip_tags} ${_sip_x} ${SIP_EXTRA_OPTIONS} -j ${SIP_CONCAT_PARTS} -c ${CMAKE_CURRENT_SIP_OUTPUT_DIR} ${_sip_includes} ${_abs_module_sip} - DEPENDS ${_abs_module_sip} ${SIP_EXTRA_FILES_DEPEND} - ) - # not sure if type MODULE could be uses anywhere, limit to cygwin for now - IF (CYGWIN OR APPLE) - ADD_LIBRARY(${_logical_name} MODULE ${_sip_output_files} ) - ELSE (CYGWIN OR APPLE) - ADD_LIBRARY(${_logical_name} SHARED ${_sip_output_files} ) - ENDIF (CYGWIN OR APPLE) - TARGET_LINK_LIBRARIES(${_logical_name} ${PYTHON_LIBRARY}) - TARGET_LINK_LIBRARIES(${_logical_name} ${EXTRA_LINK_LIBRARIES}) - SET_TARGET_PROPERTIES(${_logical_name} PROPERTIES PREFIX "" OUTPUT_NAME ${_child_module_name}) - - INSTALL(TARGETS ${_logical_name} DESTINATION "${PYTHON_SITE_PACKAGES_INSTALL_DIR}/${_parent_module_path}") - -ENDMACRO(ADD_SIP_PYTHON_MODULE) diff --git a/python_orocos_kdl/doc/conf.py b/python_orocos_kdl/doc/conf.py index 085d5d5a7..a7a4e1fa3 100644 --- a/python_orocos_kdl/doc/conf.py +++ b/python_orocos_kdl/doc/conf.py @@ -13,6 +13,8 @@ import sys, os +import PyKDL as kdl + # If extensions (or modules to document with autodoc) are in another directory, # add these directories to sys.path here. If the directory is relative to the # documentation root, use os.path.abspath to make it absolute, like shown here. @@ -45,9 +47,9 @@ # built documents. # # The short X.Y version. -version = '1.4' +version = '.'.join(kdl.__version__.split('.')[:1]) # The full version, including alpha/beta/rc tags. -release = '1.4.0' +release = kdl.__version__ # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. @@ -209,5 +211,5 @@ # (source start file, name, description, authors, manual section). man_pages = [ ('index', 'orocoskdlpythonbindings', u'Orocos KDL python bindings Documentation', - [u'Ruben Smits'], 1) + [u'Ruben Smits', u"Matthijs van der Burgh"], 1) ] diff --git a/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.bash.in b/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.bash.in deleted file mode 120000 index 47a63df22..000000000 --- a/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.bash.in +++ /dev/null @@ -1 +0,0 @@ -python_orocos_kdl_site_packages.sh.in \ No newline at end of file diff --git a/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.sh.in b/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.sh.in deleted file mode 100644 index 13d938d3c..000000000 --- a/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.sh.in +++ /dev/null @@ -1 +0,0 @@ -export PYTHONPATH=@PYTHON_SITE_PACKAGES_INSTALL_DIR@:$PYTHONPATH diff --git a/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.tcsh.in b/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.tcsh.in deleted file mode 120000 index 47a63df22..000000000 --- a/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.tcsh.in +++ /dev/null @@ -1 +0,0 @@ -python_orocos_kdl_site_packages.sh.in \ No newline at end of file diff --git a/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.zsh.in b/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.zsh.in deleted file mode 120000 index 47a63df22..000000000 --- a/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.zsh.in +++ /dev/null @@ -1 +0,0 @@ -python_orocos_kdl_site_packages.sh.in \ No newline at end of file diff --git a/python_orocos_kdl/mainpage.dox b/python_orocos_kdl/mainpage.dox deleted file mode 100644 index 0be6e3112..000000000 --- a/python_orocos_kdl/mainpage.dox +++ /dev/null @@ -1,26 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b python_orocos_kdl is ... - - - - -\section codeapi Code API - - - - -*/ diff --git a/python_orocos_kdl/manifest.xml b/python_orocos_kdl/manifest.xml deleted file mode 100644 index ae3cd5ac7..000000000 --- a/python_orocos_kdl/manifest.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - python_orocos_kdl - - - Ruben Smits - LGPL - - http://ros.org/wiki/python_orocos_kdl - - - - - - - - - - - - diff --git a/python_orocos_kdl/package.xml b/python_orocos_kdl/package.xml index 3fe99b324..89ae34009 100644 --- a/python_orocos_kdl/package.xml +++ b/python_orocos_kdl/package.xml @@ -1,25 +1,35 @@ - + + + python_orocos_kdl - 1.4.0 + 1.5.1 This package contains the python bindings PyKDL for the Kinematics and Dynamics - Library (KDL), distributed by the Orocos Project. + Library (KDL), distributed by the Orocos Project. - Ruben Smits + Ruben Smits + Matthijs van der Burgh http://wiki.ros.org/python_orocos_kdl - LGPL + LGPL-2.1-or-later - cmake + catkin orocos_kdl - python-sip - catkin - orocos_kdl - python-sip + python3 + + catkin + orocos_kdl + + python3-future + python3-psutil + + python3-sphinx - cmake + diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 new file mode 160000 index 000000000..8b03ffa7c --- /dev/null +++ b/python_orocos_kdl/pybind11 @@ -0,0 +1 @@ +Subproject commit 8b03ffa7c06cd9c8a38297b1c8923695d1ff1b07 diff --git a/python_orocos_kdl/rosdoc.yaml b/python_orocos_kdl/rosdoc.yaml index daa8282ec..b2f4d1f9e 100644 --- a/python_orocos_kdl/rosdoc.yaml +++ b/python_orocos_kdl/rosdoc.yaml @@ -1,3 +1,4 @@ - builder: sphinx output_dir: doc - sphinx_root_dir: doc \ No newline at end of file + sphinx_root_dir: doc + name: Python API diff --git a/python_orocos_kdl/tests/PyKDLtest.py b/python_orocos_kdl/tests/PyKDLtest.py index 37375da10..af5e80e31 100644 --- a/python_orocos_kdl/tests/PyKDLtest.py +++ b/python_orocos_kdl/tests/PyKDLtest.py @@ -1,9 +1,10 @@ -#!/usr/bin/python -# Copyright (C) 2007 Ruben Smits +#! /usr/bin/env python +# Copyright (C) 2007 Ruben Smits # Version: 1.0 -# Author: Ruben Smits -# Maintainer: Ruben Smits +# Author: Ruben Smits +# Maintainer: Ruben Smits +# Maintainer: Matthijs van der Burgh # URL: http://www.orocos.org/kdl # This library is free software; you can redistribute it and/or @@ -19,14 +20,25 @@ # You should have received a copy of the GNU Lesser General Public # License along with this library; if not, write to the Free Software # Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + import unittest +import dynamicstest import kinfamtest import framestest import frameveltest suite = unittest.TestSuite() +suite.addTest(dynamicstest.suite()) suite.addTest(framestest.suite()) suite.addTest(frameveltest.suite()) suite.addTest(kinfamtest.suite()) -unittest.TextTestRunner(verbosity=3).run(suite) +if __name__ == "__main__": + import sys + result = unittest.TextTestRunner(verbosity=3).run(suite) + + if result.wasSuccessful(): + sys.exit(0) + else: + sys.exit(1) diff --git a/python_orocos_kdl/tests/dynamicstest.py b/python_orocos_kdl/tests/dynamicstest.py new file mode 100644 index 000000000..5d0169c33 --- /dev/null +++ b/python_orocos_kdl/tests/dynamicstest.py @@ -0,0 +1,76 @@ +# Copyright (C) 2020 Matthijs van der Burgh + +# Version: 1.0 +# Author: Matthijs van der Burgh +# Maintainer: Ruben Smits +# Maintainer: Matthijs van der Burgh +# URL: http://www.orocos.org/kdl + +# This library is free software; you can redistribute it and/or +# modify it under the terms of the GNU Lesser General Public +# License as published by the Free Software Foundation; either +# version 2.1 of the License, or (at your option) any later version. + +# This library is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +# Lesser General Public License for more details. + +# You should have received a copy of the GNU Lesser General Public +# License along with this library; if not, write to the Free Software +# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +from PyKDL import * +import unittest + + +class DynamicsTestFunctions(unittest.TestCase): + def testJntSpaceInertiaMatrix(self): + ll = 3 + jm = JntSpaceInertiaMatrix(3) + # __getitem__ + for i in range(ll): + for j in range(ll): + self.assertEqual(jm[i, j], 0) + with self.assertRaises(IndexError): + _ = jm[-1, 0] + with self.assertRaises(IndexError): + _ = jm[3, 0] + with self.assertRaises(IndexError): + _ = jm[2, -1] + with self.assertRaises(IndexError): + _ = jm[2, 3] + + # __setitem__ + for i in range(ll): + for j in range(ll): + jm[i, j] = 3 * i + j + for i in range(ll): + for j in range(ll): + self.assertEqual(jm[i, j], 3 * i + j) + with self.assertRaises(IndexError): + jm[-1, 0] = 1 + with self.assertRaises(IndexError): + jm[3, 0] = 1 + with self.assertRaises(IndexError): + jm[2, -1] = 1 + with self.assertRaises(IndexError): + jm[2, 3] = 1 + + +def suite(): + suite = unittest.TestSuite() + suite.addTest(DynamicsTestFunctions('testJntSpaceInertiaMatrix')) + return suite + + +if __name__ == '__main__': + import sys + suite = suite() + result = unittest.TextTestRunner(verbosity=3).run(suite) + + if result.wasSuccessful(): + sys.exit(0) + else: + sys.exit(1) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index 41429dcf4..cd6abe1af 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -1,8 +1,10 @@ -# Copyright (C) 2007 Ruben Smits +# Copyright (C) 2007 Ruben Smits # Version: 1.0 -# Author: Ruben Smits -# Maintainer: Ruben Smits +# Author: Ruben Smits +# Author: Matthijs van der Burgh +# Maintainer: Ruben Smits +# Maintainer: Matthijs van der Burgh # URL: http://www.orocos.org/kdl # This library is free software; you can redistribute it and/or @@ -20,174 +22,484 @@ # Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -import unittest +from math import pi, radians, sqrt from PyKDL import * -from math import * - +import sys +import unittest + + class FramesTestFunctions(unittest.TestCase): + def testVector(self): + v = Vector(3, 4, 5) + self.testVectorImpl(v) + v = Vector() + self.testVectorImpl(v) + + # Equality + v = Vector(3, 4, 5) + self.assertFalse(v == -v) # Doesn't work for zero vector + self.assertFalse(Equal(v, -v)) # Doesn't work for zero vector + self.assertTrue(v != -v) # Doesn't work for zero vector + self.assertTrue(not Equal(v, -v)) # Doesn't work for zero vector + + # Hash + self.assertEqual(hash(v), 3450679443808348711) + self.assertEqual(hash(Vector()), 11093822414574) - def testVector2(self,v): - self.assertEqual(2*v-v,v) - self.assertEqual(v*2-v,v) - self.assertEqual(v+v+v-2*v,v) - v2=Vector(v) - self.assertEqual(v,v2) - v2+=v - self.assertEqual(2*v,v2) - v2-=v - self.assertEqual(v,v2) + # Test member get and set functions + self.assertEqual(v.x(), 3) + v.x(1) + self.assertEqual(v, Vector(1, 4, 5)) + self.assertEqual(v.y(), 4) + v.y(1) + self.assertEqual(v, Vector(1, 1, 5)) + self.assertEqual(v.z(), 5) + v.z(1) + self.assertEqual(v, Vector(1, 1, 1)) + + # __getitem__ + self.assertEqual(v[0], 1) + self.assertEqual(v[1], 1) + self.assertEqual(v[2], 1) + with self.assertRaises(IndexError): + _ = v[-1] + with self.assertRaises(IndexError): + _ = v[3] + + # __setitem__ + v[0] = 3 + self.assertEqual(v[0], 3) + v[1] = 4 + self.assertEqual(v[1], 4) + v[2] = 5 + self.assertEqual(v[2], 5) + with self.assertRaises(IndexError): + v[-1] = 1 + with self.assertRaises(IndexError): + v[3] = 1 + + # Zero + SetToZero(v) + self.assertEqual(v, Vector(0, 0, 0)) + self.assertEqual(Vector.Zero(), Vector(0, 0, 0)) + + def testVectorImpl(self, v): + self.assertTrue(v == v) + self.assertTrue(Equal(v, v)) + self.assertFalse(v != v) + self.assertFalse(not Equal(v, v)) + self.assertEqual(2*v-v, v) + self.assertEqual(v*2-v, v) + self.assertEqual(v+v+v-2*v, v) + v2 = Vector(v) + self.assertEqual(v, v2) + self.assertEqual(hash(v), hash(v2)) + v2 += v + self.assertEqual(2*v, v2) + v2 -= v + self.assertEqual(v, v2) v2.ReverseSign() - self.assertEqual(v,-v2) + self.assertEqual(v, -v2) + for v2 in [Vector(v), -Vector(v)]: + self.assertAlmostEqual(v2.Norm()**2, sum(map(lambda i: i * i, v2)), delta=1e-10) + self.assertEqual(v2.Norm(), v2.Normalize()) # Norm before Normalize, so taking norm of un-normalized vector - def testVector(self): - v=Vector(3,4,5) - self.testVector2(v) - v=Vector.Zero() - self.testVector2(v) - - def testTwist2(self,t): - self.assertEqual(2*t-t,t) - self.assertEqual(t*2-t,t) - self.assertEqual(t+t+t-2*t,t) - t2=Twist(t) - self.assertEqual(t,t2) - t2+=t - self.assertEqual(2*t,t2) - t2-=t - self.assertEqual(t,t2) - t.ReverseSign() - self.assertEqual(t,-t2) + self.assertEqual(dot(v, v), sum(map(lambda i: i * i, v))) def testTwist(self): - t=Twist(Vector(6,3,5),Vector(4,-2,7)) - self.testTwist2(t) - t=Twist.Zero() - self.testTwist2(t) - t=Twist(Vector(0,-9,-3),Vector(1,-2,-4)) - - def testWrench2(self,w): - self.assertEqual(2*w-w,w) - self.assertEqual(w*2-w,w) - self.assertEqual(w+w+w-2*w,w) - w2=Wrench(w) - self.assertEqual(w,w2) - w2+=w - self.assertEqual(2*w,w2) - w2-=w - self.assertEqual(w,w2) - w.ReverseSign() - self.assertEqual(w,-w2) + t = Twist(Vector(6, 3, 5), Vector(4, -2, 7)) + self.testTwistImpl(t) + t = Twist() + self.testTwistImpl(t) + t = Twist(Vector(0, -9, -3), Vector(1, -2, -4)) + self.testTwistImpl(t) + + # Equality + t = Twist(Vector(0, -9, -3), Vector(1, -2, -4)) + self.assertFalse(t == -t) # Doesn't work for zero twist + self.assertFalse(Equal(t, -t)) # Doesn't work for zero twist + self.assertTrue(t != -t) # Doesn't work for zero twist + self.assertTrue(not Equal(t, -t)) # Doesn't work for zero twist + + # Hash + self.assertEqual(hash(t), 3373832976806748309) + self.assertEqual(hash(Twist()), 730713428471863) + + # Members + v1 = Vector(1, 2, 3) + v2 = Vector(4, 5, 6) + t = Twist(v1, v2) + self.assertEqual(t.vel, v1) + self.assertEqual(t.rot, v2) + self.assertEqual(Twist(t).vel, t.vel) + self.assertEqual(Twist(t).rot, t.rot) + + # __getitem__ + for i in range(6): + self.assertEqual(t[i], i+1) + with self.assertRaises(IndexError): + _ = t[-1] + with self.assertRaises(IndexError): + _ = t[6] + + # __setitem__ + for i in range(6): + t[i] = i + for i in range(6): + self.assertEqual(t[i], i) + with self.assertRaises(IndexError): + t[-1] = 1 + with self.assertRaises(IndexError): + t[6] = 1 + + # Zero + SetToZero(t) + self.assertEqual(t, Twist()) + self.assertEqual(Twist.Zero(), Twist()) + + def testTwistImpl(self, t): + self.assertTrue(t == t) + self.assertTrue(Equal(t, t)) + self.assertFalse(t != t) + self.assertFalse(not Equal(t, t)) + self.assertEqual(2*t-t, t) + self.assertEqual(t*2-t, t) + self.assertEqual(t+t+t-2*t, t) + t2 = Twist(t) + self.assertEqual(t, t2) + self.assertEqual(hash(t), hash(t2)) + t2 += t + self.assertEqual(2*t, t2) + t2 -= t + self.assertEqual(t, t2) + t2.ReverseSign() + self.assertEqual(t, -t2) + v = Vector(1, 2, 3) + t2 = t.RefPoint(v) + self.assertEqual(t2.vel, t.vel + t.rot*v) + self.assertEqual(t2.rot, t.rot) + + # No need to test the dot functions again for Wrench + w = Wrench(v, v) + dot_result = dot(t.vel, w.force) + dot(t.rot, w.torque) + self.assertEqual(dot(t, w), dot_result) + self.assertEqual(dot(w, t), dot_result) def testWrench(self): - w=Wrench(Vector(7,-1,3),Vector(2,-3,3)) - self.testWrench2(w) - w=Wrench.Zero() - self.testWrench2(w) - w=Wrench(Vector(2,1,4),Vector(5,3,1)) - self.testWrench2(w) - - def testRotation2(self,v,a,b,c): - w=Wrench(Vector(7,-1,3),Vector(2,-3,3)) - t=Twist(Vector(6,3,5),Vector(4,-2,7)) - R=Rotation.RPY(a,b,c) - - self.assertAlmostEqual(dot(R.UnitX(),R.UnitX()),1.0,15) - self.assertEqual(dot(R.UnitY(),R.UnitY()),1.0) - self.assertEqual(dot(R.UnitZ(),R.UnitZ()),1.0) - self.assertAlmostEqual(dot(R.UnitX(),R.UnitY()),0.0,15) - self.assertAlmostEqual(dot(R.UnitX(),R.UnitZ()),0.0,15) - self.assertEqual(dot(R.UnitY(),R.UnitZ()),0.0) - R2=Rotation(R) - self.assertEqual(R,R2) - self.assertAlmostEqual((R*v).Norm(),v.Norm(),14) - self.assertEqual(R.Inverse(R*v),v) - self.assertEqual(R.Inverse(R*t),t) - self.assertEqual(R.Inverse(R*w),w) - self.assertEqual(R*R.Inverse(v),v) - self.assertEqual(R*Rotation.Identity(),R) - self.assertEqual(Rotation.Identity()*R,R) - self.assertEqual(R*(R*(R*v)),(R*R*R)*v) - self.assertEqual(R*(R*(R*t)),(R*R*R)*t) - self.assertEqual(R*(R*(R*w)),(R*R*R)*w) - self.assertEqual(R*R.Inverse(),Rotation.Identity()) - self.assertEqual(R.Inverse()*R,Rotation.Identity()) - self.assertEqual(R.Inverse()*v,R.Inverse(v)) - (ra,rb,rc)=R.GetRPY() - self.assertEqual(ra,a) - self.assertEqual(rb,b) - self.assertEqual(rc,c) - R = Rotation.EulerZYX(a,b,c) - (ra,rb,rc)=R.GetEulerZYX() - self.assertEqual(ra,a) - self.assertEqual(rb,b) - self.assertEqual(rc,c) - R = Rotation.EulerZYZ(a,b,c) - (ra,rb,rc)=R.GetEulerZYZ() - self.assertEqual(ra,a) - self.assertEqual(rb,b) - self.assertAlmostEqual(rc,c,15) - (angle,v2)= R.GetRotAngle() - R2=Rotation.Rot(v2,angle) - self.assertEqual(R2,R) - R2=Rotation.Rot(v2*1E20,angle) - self.assertEqual(R,R2) - v2=Vector(6,2,4) - self.assertAlmostEqual(v2.Norm(),sqrt(dot(v2,v2)),14) - + w = Wrench(Vector(7, -1, 3), Vector(2, -3, 3)) + self.testWrenchImpl(w) + w = Wrench() + self.testWrenchImpl(w) + w = Wrench(Vector(2, 1, 4), Vector(5, 3, 1)) + self.testWrenchImpl(w) + + # Equality + self.assertFalse(w == -w) # Doesn't work for zero wrench + self.assertFalse(Equal(w, -w)) # Doesn't work for zero wrench + self.assertTrue(w != -w) # Doesn't work for zero wrench + self.assertTrue(not Equal(w, -w)) # Doesn't work for zero wrench + + # Hash + self.assertEqual(hash(w), hash(13897938943539516747)) # 551895977443887016 + self.assertEqual(hash(Wrench()), 730713428471863) + + # Members + v1 = Vector(1, 2, 3) + v2 = Vector(4, 5, 6) + w = Wrench(v1, v2) + self.assertEqual(w.force, v1) + self.assertEqual(w.torque, v2) + self.assertEqual(Wrench(w).force, w.force) + self.assertEqual(Wrench(w).torque, w.torque) + + # __getitem__ + for i in range(6): + self.assertEqual(w[i], i+1) + with self.assertRaises(IndexError): + _ = w[-1] + with self.assertRaises(IndexError): + _ = w[6] + + # __setitem__ + for i in range(6): + w[i] = i + for i in range(6): + self.assertEqual(w[i], i) + with self.assertRaises(IndexError): + w[-1] = 1 + with self.assertRaises(IndexError): + w[6] = 1 + + # Zero + SetToZero(w) + self.assertEqual(w, Wrench()) + self.assertEqual(Wrench.Zero(), Wrench()) + + def testWrenchImpl(self, w): + self.assertEqual(2*w-w, w) + self.assertEqual(w*2-w, w) + self.assertEqual(w+w+w-2*w, w) + w2 = Wrench(w) + self.assertEqual(w, w2) + self.assertEqual(hash(w), hash(w2)) + w2 += w + self.assertEqual(2*w, w2) + w2 -= w + self.assertEqual(w, w2) + w2.ReverseSign() + self.assertEqual(w, -w2) + + v = Vector(1, 2, 3) + w2 = w.RefPoint(v) + self.assertEqual(w2.force, w.force) + self.assertEqual(w2.torque, w.torque + w.force*v) + def testRotation(self): - self.testRotation2(Vector(3,4,5),radians(10),radians(20),radians(30)) - + self.testRotationImpl(Rotation.RPY(radians(10), radians(20), radians(30)), Vector(3, 4, 5)) + self.testRotationImpl(Rotation.RPY(radians(10), radians(20), radians(30)), Vector()) + self.testRotationImpl(Rotation(), Vector(3, 4, 5)) + self.testRotationImpl(Rotation(), Vector()) + + # Hash + self.assertEqual(hash(Rotation.Quaternion(1, 0, 0, 0)), 5526237894416316219) + self.assertEqual(hash(Rotation()), 8386870752212395617) + + r = Rotation(*range(1, 10)) + + # __getitem__ + for i in range(3): + for j in range(3): + self.assertEqual(r[i, j], 3*i+j+1) + with self.assertRaises(IndexError): + _ = r[-1, 0] + with self.assertRaises(IndexError): + _ = r[0, -1] + with self.assertRaises(IndexError): + _ = r[3, 2] + with self.assertRaises(IndexError): + _ = r[2, 3] + + # __setitem__ + for i in range(3): + for j in range(3): + r[i, j] = 3*i+j + for i in range(3): + for j in range(3): + self.assertEqual(r[i, j], 3*i+j) + with self.assertRaises(IndexError): + r[-1, 0] = 1 + with self.assertRaises(IndexError): + r[0, -1] = 1 + with self.assertRaises(IndexError): + r[3, 2] = 1 + with self.assertRaises(IndexError): + r[2, 3] = 1 + + def testRotationImpl(self, r, v): + w = Wrench(Vector(7, -1, 3), Vector(2, -3, 3)) + t = Twist(Vector(6, 3, 5), Vector(4, -2, 7)) + a, b, c = r.GetRPY() + + self.assertAlmostEqual(dot(r.UnitX(), r.UnitX()), 1.0, 15) + self.assertEqual(dot(r.UnitY(), r.UnitY()), 1.0) + self.assertEqual(dot(r.UnitZ(), r.UnitZ()), 1.0) + self.assertAlmostEqual(dot(r.UnitX(), r.UnitY()), 0.0, 15) + self.assertAlmostEqual(dot(r.UnitX(), r.UnitZ()), 0.0, 15) + self.assertAlmostEqual(dot(r.UnitY(), r.UnitZ()), 0.0, 15) + r2 = Rotation(r) + self.assertEqual(r, r2) + self.assertEqual(hash(r), hash(r2)) + self.assertAlmostEqual((r*v).Norm(), v.Norm(), 14) + self.assertEqual(r.Inverse(r*v), v) + self.assertEqual(r.Inverse(r*t), t) + self.assertEqual(r.Inverse(r*w), w) + self.assertEqual(r*r.Inverse(v), v) + self.assertEqual(r*Rotation.Identity(), r) + self.assertEqual(Rotation.Identity()*r, r) + self.assertEqual(r*(r*(r*v)), (r*r*r)*v) + self.assertEqual(r*(r*(r*t)), (r*r*r)*t) + self.assertEqual(r*(r*(r*w)), (r*r*r)*w) + self.assertEqual(r*r.Inverse(), Rotation.Identity()) + self.assertEqual(r.Inverse()*r, Rotation.Identity()) + self.assertEqual(r.Inverse()*v, r.Inverse(v)) + (ra, rb, rc) = r.GetRPY() + self.assertEqual(ra, a) + self.assertEqual(rb, b) + self.assertEqual(rc, c) + r = Rotation.EulerZYX(a, b, c) + (ra, rb, rc) = r.GetEulerZYX() + self.assertEqual(ra, a) + self.assertEqual(rb, b) + self.assertEqual(rc, c) + r = Rotation.EulerZYZ(a, b, c) + (ra, rb, rc) = r.GetEulerZYZ() + self.assertEqual(ra, a) + self.assertEqual(rb, b) + self.assertAlmostEqual(rc, c, 15) + (angle, v2) = r.GetRotAngle() + r2 = Rotation.Rot(v2, angle) + self.assertEqual(r2, r) + r2 = Rotation.Rot(v2*1E20, angle) + self.assertEqual(r, r2) + v2 = Vector(6, 2, 4) + self.assertAlmostEqual(v2.Norm(), sqrt(dot(v2, v2)), 14) + def testFrame(self): - v=Vector(3,4,5) - w=Wrench(Vector(7,-1,3),Vector(2,-3,3)) - t=Twist(Vector(6,3,5),Vector(4,-2,7)) - F = Frame(Rotation.EulerZYX(radians(10),radians(20),radians(-10)),Vector(4,-2,1)) - F2=Frame(F) - self.assertEqual(F,F2) - self.assertEqual(F.Inverse(F*v),v) - self.assertEqual(F.Inverse(F*t),t) - self.assertEqual(F.Inverse(F*w),w) - self.assertEqual(F*F.Inverse(v),v) - self.assertEqual(F*F.Inverse(t),t) - self.assertEqual(F*F.Inverse(w),w) - self.assertEqual(F*Frame.Identity(),F) - self.assertEqual(Frame.Identity()*F,F) - self.assertEqual(F*(F*(F*v)),(F*F*F)*v) - self.assertEqual(F*(F*(F*t)),(F*F*F)*t) - self.assertEqual(F*(F*(F*w)),(F*F*F)*w) - self.assertEqual(F*F.Inverse(),Frame.Identity()) - self.assertEqual(F.Inverse()*F,Frame.Identity()) - self.assertEqual(F.Inverse()*v,F.Inverse(v)) + v = Vector(3, 4, 5) + w = Wrench(Vector(7, -1, 3), Vector(2, -3, 3)) + t = Twist(Vector(6, 3, 5), Vector(4, -2, 7)) + f = Frame() + self.testFrameImpl(v, w, t, f) + r = Rotation.EulerZYX(radians(10), radians(20), radians(-10)) + v2 = Vector(4, -2, 1) + f = Frame(r, v2) + self.testFrameImpl(v, w, t, f) + + # Equality + f2 = Frame(f) + self.assertEqual(f, f2) + self.assertEqual(hash(f), hash(f2)) + + # Hash + self.assertEqual(hash(f), 6112004106257185417) + self.assertEqual(hash(Frame()), 8387572672274540708) + + # Members + self.assertEqual(f.M, r) + self.assertEqual(f.p, v2) + self.assertEqual(Frame(f).M, f.M) + self.assertEqual(Frame(f).p, f.p) + + # Denavit-Hartenberg + f_dh = Frame(Rotation(1, 0, 0, + 0, 0, -1, + 0, 1, 0), + Vector(0, 0, 0.36)) + self.assertTrue(Equal(Frame.DH(0.0, pi/2, 0.36, 0.0), f_dh)) + # Only for testing purposes, shouldn't use static function of instances + self.assertTrue(Equal(Frame().DH(0.0, pi/2, 0.36, 0.0), f_dh)) + + f_dh_craig1989 = Frame(Rotation(1, 0, 0, + 0, 0, -1, + 0, 1, 0), + Vector(0, -0.36, 0)) + self.assertTrue(Equal(Frame.DH_Craig1989(0.0, pi/2, 0.36, 0.0), f_dh_craig1989)) + # Only for testing purposes, shouldn't use static function of instances + self.assertTrue(Equal(Frame().DH_Craig1989(0.0, pi / 2, 0.36, 0.0), f_dh_craig1989)) + + f = Frame(Rotation(1, 2, 3, + 5, 6, 7, + 9, 10, 11), + Vector(4, 8, 12)) + # __getitem__ + for i in range(3): + for j in range(4): + self.assertEqual(f[i, j], 4*i+j+1) + with self.assertRaises(IndexError): + _ = f[-1, 0] + with self.assertRaises(IndexError): + _ = f[0, -1] + with self.assertRaises(IndexError): + _ = f[3, 3] + with self.assertRaises(IndexError): + _ = f[2, 4] + + # __setitem__ + for i in range(3): + for j in range(4): + f[i, j] = 4*i+j + for i in range(3): + for j in range(4): + self.assertEqual(f[i, j], 4*i+j) + with self.assertRaises(IndexError): + f[-1, 0] = 1 + with self.assertRaises(IndexError): + f[0, -1] = 1 + with self.assertRaises(IndexError): + f[3, 3] = 1 + with self.assertRaises(IndexError): + f[2, 4] = 1 + + def testFrameImpl(self, v, w, t, f): + self.assertEqual(f.Inverse(f*v), v) + self.assertEqual(f.Inverse(f*t), t) + self.assertEqual(f.Inverse(f*w), w) + self.assertEqual(f*f.Inverse(v), v) + self.assertEqual(f*f.Inverse(t), t) + self.assertEqual(f*f.Inverse(w), w) + self.assertEqual(f*Frame.Identity(), f) + self.assertEqual(Frame.Identity()*f, f) + self.assertEqual(f*(f*(f*v)), (f*f*f)*v) + self.assertEqual(f*(f*(f*t)), (f*f*f)*t) + self.assertEqual(f*(f*(f*w)), (f*f*f)*w) + self.assertEqual(f*f.Inverse(), Frame.Identity()) + self.assertEqual(f.Inverse()*f, Frame.Identity()) + self.assertEqual(f.Inverse()*v, f.Inverse(v)) def testPickle(self): - import pickle - data = {} - data['v'] = Vector(1,2,3) - data['rot'] = Rotation.RotX(1.3) + if sys.version_info < (3, 0): + import cPickle as pickle + else: + import pickle + data = dict() + data['v'] = Vector(1, 2, 3) + data['rot'] = Rotation().EulerZYZ(1, 2, 3) data['fr'] = Frame(data['rot'], data['v']) - data['tw'] = Twist(data['v'], Vector(4,5,6)) - data['wr'] = Wrench(Vector(0.1,0.2,0.3), data['v']) - - f = open('/tmp/pickle_test', 'w') - pickle.dump(data, f) - f.close() - - f = open('/tmp/pickle_test', 'r') - data1 = pickle.load(f) - f.close() - + data['tw'] = Twist(data['v'], Vector(4, 5, 6)) + data['wr'] = Wrench(Vector(0.1, 0.2, 0.3), data['v']) + + with open('/tmp/pickle_test', 'wb') as f: + pickle.dump(data, f, 2) + + with open('/tmp/pickle_test', 'rb') as f: + data1 = pickle.load(f) + self.assertEqual(data, data1) + def testCopyImpl(self, copy): + v1 = Vector(1, 2, 3) + v2 = copy(v1) + self.assertEqual(v1, v2) + r1 = Rotation().EulerZYZ(1, 2, 3) + r2 = copy(r1) + self.assertEqual(r1, r2) + f1 = Frame(r1, v1) + f2 = copy(f1) + self.assertEqual(f1, f2) + t1 = Twist(v1, Vector(4, 5, 6)) + t2 = copy(t1) + self.assertEqual(t1, t2) + w1 = Wrench(Vector(0.1, 0.2, 0.3), v1) + w2 = copy(w1) + self.assertEqual(w1, w2) + + def testCopy(self): + from copy import copy + self.testCopyImpl(copy) + + def testDeepCopy(self): + from copy import deepcopy + self.testCopyImpl(deepcopy) + def suite(): - suite=unittest.TestSuite() + suite = unittest.TestSuite() suite.addTest(FramesTestFunctions('testVector')) suite.addTest(FramesTestFunctions('testTwist')) suite.addTest(FramesTestFunctions('testWrench')) suite.addTest(FramesTestFunctions('testRotation')) suite.addTest(FramesTestFunctions('testFrame')) suite.addTest(FramesTestFunctions('testPickle')) + suite.addTest(FramesTestFunctions('testCopy')) + suite.addTest(FramesTestFunctions('testDeepCopy')) return suite - -#suite = suite() -#unittest.TextTestRunner(verbosity=3).run(suite) + + +if __name__ == '__main__': + suite = suite() + result = unittest.TextTestRunner(verbosity=3).run(suite) + + if result.wasSuccessful(): + sys.exit(0) + else: + sys.exit(1) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index ae0e89051..c6ffede12 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -1,203 +1,330 @@ -import unittest +# Copyright (C) 2007 Ruben Smits + +# Version: 1.0 +# Author: Ruben Smits +# Author: Matthijs van der Burgh +# Maintainer: Ruben Smits +# Maintainer: Matthijs van der Burgh +# URL: http://www.orocos.org/kdl + +# This library is free software; you can redistribute it and/or +# modify it under the terms of the GNU Lesser General Public +# License as published by the Free Software Foundation; either +# version 2.1 of the License, or (at your option) any later version. + +# This library is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +# Lesser General Public License for more details. + +# You should have received a copy of the GNU Lesser General Public +# License along with this library; if not, write to the Free Software +# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +from math import radians from PyKDL import * -from math import * +import unittest + class FrameVelTestFunctions(unittest.TestCase): + def testdoubleVel(self): + d = doubleVel() + self.assertEqual(doubleVel(d), d) + self.assertEqual(hash(d), hash(doubleVel(d))) + self.assertEqual(d.t, 0) + self.assertEqual(d.grad, 0) + self.assertEqual(d.value(), 0) + self.assertEqual(d.deriv(), 0) + d2 = -d + self.assertEqual(d2.t, -d.t) + self.assertEqual(d2.grad, -d.grad) def testVectorVel(self): - v=VectorVel(Vector(3,-4,5),Vector(6,3,-5)) - vt = Vector(-4,-6,-8); - self.assertTrue(Equal( 2*v-v,v)) - self.assertTrue(Equal( v*2-v,v)) - self.assertTrue(Equal( v+v+v-2*v,v)) - v2=VectorVel(v) - self.assertTrue(Equal( v,v2)) - v2+=v - self.assertTrue(Equal( 2*v,v2)) - v2-=v - self.assertTrue(Equal( v,v2)) + v = VectorVel() + vt = Vector() + self.testVectorVelImpl(v, vt) + vt = Vector(-4, -6, -8) + self.testVectorVelImpl(v, vt) + v1 = Vector(3, -4, 5) + v2 = Vector(6, 3, -5) + v = VectorVel(v1, v2) + self.testVectorVelImpl(v, vt) + + # Members + self.assertEqual(v.p, v1) + self.assertEqual(v.v, v2) + self.assertEqual(v.value(), v1) + self.assertEqual(v.deriv(), v2) + + # Equality + self.assertEqual(VectorVel(v).p, v.p) + self.assertEqual(VectorVel(v).v, v.v) + self.assertFalse(v == -v) # Doesn't work for zero VectorVel + self.assertFalse(Equal(v, -v)) # Doesn't work for zero VectorVel + self.assertTrue(v != -v) # Doesn't work for zero VectorVel + self.assertTrue(not Equal(v, -v)) # Doesn't work for zero VectorVel + + # Hash + self.assertEqual(hash(v), 2049006275376269995) + + self.assertEqual(hash(VectorVel()), 730713428471863) + + v = VectorVel(v1) + self.assertEqual(v, v1) + self.assertEqual(v1, v) + + # Zero + v = VectorVel(v1, v2) + SetToZero(v) + self.assertEqual(v, VectorVel()) + self.assertEqual(VectorVel.Zero(), VectorVel()) + + # dot functions + v = VectorVel(v1, v2) + self.assertEqual(dot(v, v), doubleVel(dot(v.p, v.p), dot(v.p, v.v)+dot(v.v, v.p))) + dot_result = doubleVel(dot(v.p, v1), dot(v.v, v1)) + self.assertEqual(dot(v, v1), dot_result) + self.assertEqual(dot(v1, v), dot_result) + + def testVectorVelImpl(self, v, vt): + self.assertEqual(2*v-v, v) + self.assertEqual(v*2-v, v) + self.assertEqual(v+v+v-2*v, v) + v2 = VectorVel(v) + self.assertEqual(v, v2) + self.assertEqual(hash(v), hash(v2)) + v2 += v + self.assertEqual(2*v, v2) + v2 -= v + self.assertEqual(v, v2) v2.ReverseSign() - self.assertTrue(Equal( v,-v2)) - self.assertTrue(Equal( v*vt,-vt*v)) - v2 = VectorVel(Vector(-5,-6,-3),Vector(3,4,5)) - self.assertTrue(Equal( v*v2,-v2*v)) + self.assertEqual(v, -v2) + self.assertEqual(v*vt, -vt*v) + v2 = VectorVel(Vector(-5, -6, -3), Vector(3, 4, 5)) + self.assertEqual(v*v2, -v2*v) + + def testTwistVel(self): + t = TwistVel() + self.testTwistVelImpl(t) + v1 = Vector(1, 2, 3) + v2 = Vector(4, 5, 6) + vv1 = VectorVel(v1, v2) + vv2 = VectorVel(v2, v1) + t = TwistVel(vv1, vv2) + self.testTwistVelImpl(t) + + # Alternative constructor + tw1 = Twist(v1, v2) + tw2 = Twist(v2, v1) + t2 = TwistVel(tw1, tw2) + self.assertEqual(t, t2) + + # Members + self.assertEqual(t.vel, vv1) + self.assertEqual(t.rot, vv2) + self.assertEqual(t2.value(), tw1) + self.assertEqual(t2.deriv(), tw2) + self.assertEqual(t2.GetTwist(), tw1) + self.assertEqual(t2.GetTwistDot(), tw2) + + # Equality + self.assertEqual(TwistVel(t).vel, t.vel) + self.assertEqual(TwistVel(t).rot, t.rot) + self.assertFalse(t == -t) # Doesn't work for zero TwistVel + self.assertFalse(Equal(t, -t)) # Doesn't work for zero TwistVel + self.assertTrue(t != -t) # Doesn't work for zero TwistVel + self.assertTrue(not Equal(t, -t)) # Doesn't work for zero TwistVel + + t = TwistVel(VectorVel(v1), VectorVel(v2)) + t2 = Twist(v1, v2) + self.assertEqual(t, t2) + self.assertEqual(t2, t) + + # Hash + self.assertEqual(hash(t), 141466195908239803) + self.assertEqual(hash(TwistVel()), 48409940491385244) + + # Zero + SetToZero(t) + self.assertEqual(t, TwistVel()) + self.assertEqual(TwistVel.Zero(), TwistVel()) + + def testTwistVelImpl(self, t): + self.assertEqual(2*t-t, t) + self.assertEqual(t*2-t, t) + self.assertEqual(t+t+t-2*t, t) + t2 = TwistVel(t) + self.assertEqual(t, t2) + self.assertEqual(hash(t), hash(t2)) + t2 += t + self.assertEqual(2*t, t2) + t2 -= t + self.assertEqual(t, t2) + t2.ReverseSign() + self.assertEqual(t, -t2) + self.assertEqual(t*doubleVel(), doubleVel()*t) + self.assertEqual(t*doubleVel(5), doubleVel(5)*t) + self.assertEqual(t * doubleVel(3, 5), doubleVel(3, 5) * t) - def testRotationVel(self): - v=VectorVel(Vector(9,4,-2),Vector(-5,6,-2)) - vt=Vector(2,3,4) - a= radians(-15) - b= radians(20) - c= radians(-80) - R = RotationVel(Rotation.RPY(a,b,c),Vector(2,4,1)) - R2=RotationVel(R) - self.assertTrue(Equal(R,R2)) - self.assertTrue(Equal((R*v).Norm(),(v.Norm()))) - self.assertTrue(Equal(R.Inverse(R*v),v)) - self.assertTrue(Equal(R*R.Inverse(v),v)) - self.assertTrue(Equal(R*Rotation.Identity(),R)) - self.assertTrue(Equal(Rotation.Identity()*R,R)) - self.assertTrue(Equal(R*(R*(R*v)),(R*R*R)*v)) - self.assertTrue(Equal(R*(R*(R*vt)),(R*R*R)*vt)) - self.assertTrue(Equal(R*R.Inverse(),RotationVel.Identity())) - self.assertTrue(Equal(R.Inverse()*R,RotationVel.Identity())) - self.assertTrue(Equal(R.Inverse()*v,R.Inverse(v))) - #v2=v*v-2*v - #print dot(v2,v2) - #self.assertTrue(Equal((v2).Norm(),sqrt(dot(v2,v2)))) + v = VectorVel() + vt = Vector() + r = RotationVel() + self.testRotationVelImpl(r, v, vt) + v = VectorVel(Vector(9, 4, -2), Vector(-5, 6, -2)) + vt = Vector(2, 3, 4) + rot = Rotation.RPY(radians(-15), radians(20), radians(-80)) + vec = Vector(2, 4, 1) + r = RotationVel(rot, vec) + self.testRotationVelImpl(r, v, vt) + + # Members + self.assertEqual(r.R, rot) + self.assertEqual(r.w, vec) + self.assertEqual(r.value(), rot) + self.assertEqual(r.deriv(), vec) + + # Equality + self.assertEqual(RotationVel(r).R, r.R) + self.assertEqual(RotationVel(r).w, r.w) + self.assertEqual(RotationVel(rot), rot) + self.assertEqual(rot, RotationVel(rot)) + + # Hash + self.assertEqual(hash(r), 6921222406909663069) + self.assertEqual(hash(RotationVel()), 4775665235973850935) + + def testRotationVelImpl(self, r, v, vt): + r2 = RotationVel(r) + self.assertEqual(r, r2) + self.assertEqual(hash(r), hash(r2)) + self.assertEqual((r*v).Norm(), v.Norm()) + self.assertEqual(r.Inverse(r*v), v) + self.assertEqual(r*r.Inverse(v), v) + self.assertEqual(r*Rotation.Identity(), r) + self.assertEqual(Rotation.Identity()*r, r) + self.assertEqual(r*(r*(r*v)), (r*r*r)*v) + self.assertEqual(r*(r*(r*vt)), (r*r*r)*vt) + self.assertEqual(r*r.Inverse(), RotationVel.Identity()) + self.assertEqual(r.Inverse()*r, RotationVel.Identity()) + self.assertEqual(r.Inverse()*v, r.Inverse(v)) def testFrameVel(self): - v=VectorVel(Vector(3,4,5),Vector(-2,-4,-1)) - vt=Vector(-1,0,-10) - F = FrameVel(Frame(Rotation.EulerZYX(radians(10),radians(20),radians(-10)),Vector(4,-2,1)), - Twist(Vector(2,-2,-2),Vector(-5,-3,-2))) - F2=FrameVel(F) - self.assertTrue(Equal( F,F2)) - self.assertTrue(Equal( F.Inverse(F*v),v)) - self.assertTrue(Equal( F.Inverse(F*vt), vt)) - self.assertTrue(Equal( F*F.Inverse(v),v)) - self.assertTrue(Equal( F*F.Inverse(vt),vt)) - self.assertTrue(Equal( F*Frame.Identity(),F)) - self.assertTrue(Equal( Frame.Identity()*F,F)) - self.assertTrue(Equal( F*(F*(F*v)),(F*F*F)*v)) - self.assertTrue(Equal( F*(F*(F*vt)),(F*F*F)*vt)) - self.assertTrue(Equal( F*F.Inverse(),FrameVel.Identity())) - self.assertTrue(Equal( F.Inverse()*F,Frame.Identity())) - self.assertTrue(Equal( F.Inverse()*vt,F.Inverse(vt))) + v = VectorVel() + vt = Vector() + f = FrameVel() + self.testFrameVelImpl(f, v, vt) + fr_m = Rotation.EulerZYX(radians(10), radians(20), radians(-10)) + fr_p = Vector(4, -2, 1) + tw_vel = Vector(2, -2, -2) + tw_rot = Vector(-5, -3, -2) + fr = Frame(fr_m, fr_p) + tw = Twist(tw_vel, tw_rot) + f = FrameVel(fr, tw) + self.testFrameVelImpl(f, v, vt) + v = VectorVel(Vector(3, 4, 5), Vector(-2, -4, -1)) + vt = Vector(-1, 0, -10) + self.testFrameVelImpl(f, v, vt) + + # Alternative constructor + rv = RotationVel(fr_m, tw_rot) + vv = VectorVel(fr_p, tw_vel) + f2 = FrameVel(rv, vv) + self.assertEqual(f, f2) + + # Members + self.assertEqual(f.M, rv) + self.assertEqual(f.p, vv) + self.assertEqual(f2.value(), fr) + self.assertEqual(f2.deriv(), tw) + + # Equality + self.assertEqual(FrameVel(f).M, f.M) + self.assertEqual(FrameVel(f).p, f.p) + f = FrameVel(fr) + self.assertEqual(f, fr) + self.assertEqual(fr, f) + + # Hash + self.assertEqual(hash(fr), 6112004106257185417) + self.assertEqual(hash(FrameVel()), 35564562501293795) + + def testFrameVelImpl(self, f, v, vt): + f2 = FrameVel(f) + self.assertEqual(f, f2) + self.assertEqual(hash(f), hash(f2)) + self.assertEqual(f.Inverse(f*v), v) + self.assertEqual(f.Inverse(f*vt), vt) + self.assertEqual(f*f.Inverse(v), v) + self.assertEqual(f*f.Inverse(vt), vt) + self.assertEqual(f*Frame.Identity(), f) + self.assertEqual(Frame.Identity()*f, f) + self.assertEqual(f*(f*(f*v)), (f*f*f)*v) + self.assertEqual(f*(f*(f*vt)), (f*f*f)*vt) + self.assertEqual(f*f.Inverse(), FrameVel.Identity()) + self.assertEqual(f.Inverse()*f, Frame.Identity()) + self.assertEqual(f.Inverse()*vt, f.Inverse(vt)) def testPickle(self): - rot = Rotation.RotX(1.3) import pickle data = {} - data['vv'] = VectorVel(Vector(1,2,3), Vector(4,5,6)) - data['rv'] = RotationVel(rot, Vector(4.1,5.1,6.1)) + data['vv'] = VectorVel(Vector(1, 2, 3), Vector(4, 5, 6)) + data['rv'] = RotationVel(Rotation.RotX(1.3), Vector(4.1, 5.1, 6.1)) data['fv'] = FrameVel(data['rv'], data['vv']) data['tv'] = TwistVel(data['vv'], data['vv']) - - f = open('/tmp/pickle_test_kdl_framevel', 'w') - pickle.dump(data, f) - f.close() - - f = open('/tmp/pickle_test_kdl_framevel', 'r') - data1 = pickle.load(f) - f.close() - - self.assertEqual(data['vv'].p, data1['vv'].p) - self.assertEqual(data['vv'].v, data1['vv'].v) - self.assertEqual(data['rv'].R, data1['rv'].R) - self.assertEqual(data['rv'].w, data1['rv'].w) - self.assertEqual(data['fv'].M.R, data1['fv'].M.R) - self.assertEqual(data['fv'].M.w, data1['fv'].M.w) - self.assertEqual(data['fv'].p.p, data1['fv'].p.p) - self.assertEqual(data['fv'].p.v, data1['fv'].p.v) - self.assertEqual(data['tv'].vel.p, data1['tv'].vel.p) - self.assertEqual(data['tv'].vel.v, data1['tv'].vel.v) - self.assertEqual(data['tv'].rot.p, data1['tv'].rot.p) - self.assertEqual(data['tv'].rot.v, data1['tv'].rot.v) - -#void TestTwistVel() { -# KDL_CTX; -# // Twist -# TwistVel t(VectorVel( -# Vector(6,3,5), -# Vector(1,4,2) -# ),VectorVel( -# Vector(4,-2,7), -# Vector(-1,-2,-3) -# ) -# ); -# TwistVel t2; -# RotationVel R(Rotation::RPY(10*deg2rad,20*deg2rad,-15*deg2rad),Vector(-1,5,3)); -# FrameVel F = FrameVel( -# Frame( -# Rotation::EulerZYX(-17*deg2rad,13*deg2rad,-16*deg2rad), -# Vector(4,-2,1) -# ), -# Twist( -# Vector(2,-2,-2), -# Vector(-5,-3,-2) -# ) -# ); -# -# KDL_DIFF(2.0*t-t,t); -# KDL_DIFF(t*2.0-t,t); -# KDL_DIFF(t+t+t-2.0*t,t); -# t2=t; -# KDL_DIFF(t,t2); -# t2+=t; -# KDL_DIFF(2.0*t,t2); -# t2-=t; -# KDL_DIFF(t,t2); -# t.ReverseSign(); -# KDL_DIFF(t,-t2); -# KDL_DIFF(R.Inverse(R*t),t); -# KDL_DIFF(R*t,R*R.Inverse(R*t)); -# KDL_DIFF(F.Inverse(F*t),t); -# KDL_DIFF(F*t,F*F.Inverse(F*t)); -# KDL_DIFF(doubleVel(3.14,2)*t,t*doubleVel(3.14,2)); -# KDL_DIFF(t/doubleVel(3.14,2),t*(1.0/doubleVel(3.14,2))); -# KDL_DIFF(t/3.14,t*(1.0/3.14)); -# KDL_DIFF(-t,-1.0*t); -# VectorVel p1(Vector(5,1,2),Vector(4,2,1)) ; -# VectorVel p2(Vector(2,0,5),Vector(-2,7,-1)) ; -# KDL_DIFF(t.RefPoint(p1+p2),t.RefPoint(p1).RefPoint(p2)); -# KDL_DIFF(t,t.RefPoint(-p1).RefPoint(p1)); -#} -# -#void TestTwistAcc() { -# KDL_CTX; -# // Twist -# TwistAcc t( VectorAcc(Vector(6,3,5),Vector(1,4,2),Vector(5,2,1)), -# VectorAcc(Vector(4,-2,7),Vector(-1,-2,-3),Vector(5,2,9) ) -# ); -# TwistAcc t2; -# RotationAcc R(Rotation::RPY(10*deg2rad,20*deg2rad,-15*deg2rad), -# Vector(-1,5,3), -# Vector(2,1,3) -# ) ; -# FrameAcc F = FrameAcc( -# Frame(Rotation::EulerZYX(-17*deg2rad,13*deg2rad,-16*deg2rad),Vector(4,-2,1)), -# Twist(Vector(2,-2,-2),Vector(-5,-3,-2)), -# Twist(Vector(5,4,-5),Vector(12,13,17)) -# ); -# -# KDL_DIFF(2.0*t-t,t); -# KDL_DIFF(t*2.0-t,t); -# KDL_DIFF(t+t+t-2.0*t,t); -# t2=t; -# KDL_DIFF(t,t2); -# t2+=t; -# KDL_DIFF(2.0*t,t2); -# t2-=t; -# KDL_DIFF(t,t2); -# t.ReverseSign(); -# KDL_DIFF(t,-t2); -# KDL_DIFF(R.Inverse(R*t),t); -# KDL_DIFF(R*t,R*R.Inverse(R*t)); -# KDL_DIFF(F.Inverse(F*t),t); -# KDL_DIFF(F*t,F*F.Inverse(F*t)); -# KDL_DIFF(doubleAcc(3.14,2,3)*t,t*doubleAcc(3.14,2,3)); -# KDL_DIFF(t/doubleAcc(3.14,2,7),t*(1.0/doubleAcc(3.14,2,7))); -# KDL_DIFF(t/3.14,t*(1.0/3.14)); -# KDL_DIFF(-t,-1.0*t); -# VectorAcc p1(Vector(5,1,2),Vector(4,2,1),Vector(2,1,3)); -# VectorAcc p2(Vector(2,0,5),Vector(-2,7,-1),Vector(-3,2,-1)); -# KDL_DIFF(t.RefPoint(p1+p2),t.RefPoint(p1).RefPoint(p2)); -# KDL_DIFF(t,t.RefPoint(-p1).RefPoint(p1)); -#} -# + + with open('/tmp/pickle_test_kdl_framevel', 'wb') as f: + pickle.dump(data, f, 2) + + with open('/tmp/pickle_test_kdl_framevel', 'rb') as f: + data1 = pickle.load(f) + + self.assertEqual(data, data1) + + def testCopyImpl(self, copy): + vv1 = VectorVel(Vector(1, 2, 3), Vector(4, 5, 6)) + vv2 = copy(vv1) + self.assertEqual(vv1, vv2) + rv1 = RotationVel(Rotation.RotX(1.3), Vector(4.1, 5.1, 6.1)) + rv2 = copy(rv1) + self.assertEqual(rv1, rv2) + fv1 = FrameVel(rv1, vv1) + fv2 = copy(fv1) + self.assertEqual(fv1, fv2) + tv1 = TwistVel(vv1, vv1) + tv2 = copy(tv1) + self.assertEqual(tv1, tv2) + + def testCopy(self): + from copy import copy + self.testCopyImpl(copy) + + def testDeepCopy(self): + from copy import deepcopy + self.testCopyImpl(deepcopy) + def suite(): - suite=unittest.TestSuite() + suite = unittest.TestSuite() + suite.addTest(FrameVelTestFunctions('testdoubleVel')) suite.addTest(FrameVelTestFunctions('testVectorVel')) + suite.addTest(FrameVelTestFunctions('testTwistVel')) suite.addTest(FrameVelTestFunctions('testRotationVel')) suite.addTest(FrameVelTestFunctions('testFrameVel')) suite.addTest(FrameVelTestFunctions('testPickle')) + suite.addTest(FrameVelTestFunctions('testCopy')) + suite.addTest(FrameVelTestFunctions('testDeepCopy')) return suite -#suite = suite() -#unittest.TextTestRunner(verbosity=5).run(suite) - +if __name__ == '__main__': + import sys + suite = suite() + result = unittest.TextTestRunner(verbosity=3).run(suite) + if result.wasSuccessful(): + sys.exit(0) + else: + sys.exit(1) diff --git a/python_orocos_kdl/tests/kinfamtest.py b/python_orocos_kdl/tests/kinfamtest.py index 19f39c0f5..9300bea46 100644 --- a/python_orocos_kdl/tests/kinfamtest.py +++ b/python_orocos_kdl/tests/kinfamtest.py @@ -1,8 +1,10 @@ -# Copyright (C) 2007 Ruben Smits +# Copyright (C) 2007 Ruben Smits # Version: 1.0 -# Author: Ruben Smits -# Maintainer: Ruben Smits +# Author: Ruben Smits +# Author: Matthijs van der Burgh +# Maintainer: Ruben Smits +# Maintainer: Matthijs van der Burgh # URL: http://www.orocos.org/kdl # This library is free software; you can redistribute it and/or @@ -20,138 +22,352 @@ # Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -import unittest +from builtins import range + +import gc +import psutil from PyKDL import * -from math import * import random +import unittest + + +def changeJacRepresentation(J, F_bs_ee, representation): + if representation == ChainJntToJacDotSolver.HYBRID: + pass + elif representation == ChainJntToJacDotSolver.BODYFIXED: + J.changeBase(F_bs_ee.M.Inverse()) + elif representation == ChainJntToJacDotSolver.INERTIAL: + J.changeRefPoint(-F_bs_ee.p) + + +def Jdot_diff(J_q, J_qdt, dt, Jdot): + assert J_q.columns() == J_qdt.columns() + assert J_q.columns() == Jdot.columns() + for l in range(J_q.rows()): + for c in range(J_q.columns()): + Jdot[l, c] = (J_qdt[l, c] - J_q[l, c]) / dt + class KinfamTestFunctions(unittest.TestCase): - def setUp(self): self.chain = Chain() self.chain.addSegment(Segment(Joint(Joint.RotZ), - Frame(Vector(0.0,0.0,0.0)))) + Frame(Vector(0.0, 0.0, 0.0)))) self.chain.addSegment(Segment(Joint(Joint.RotX), - Frame(Vector(0.0,0.0,0.9)))) - self.chain.addSegment(Segment(Joint(Joint.None), - Frame(Vector(-0.4,0.0,0.0)))) + Frame(Vector(0.0, 0.0, 0.9)))) + self.chain.addSegment(Segment(Joint(Joint.Fixed), + Frame(Vector(-0.4, 0.0, 0.0)))) self.chain.addSegment(Segment(Joint(Joint.RotY), - Frame(Vector(0.0,0.0,1.2)))) - self.chain.addSegment(Segment(Joint(Joint.None), - Frame(Vector(0.4,0.0,0.0)))) + Frame(Vector(0.0, 0.0, 1.2)))) + self.chain.addSegment(Segment(Joint(Joint.Fixed), + Frame(Vector(0.4, 0.0, 0.0)))) self.chain.addSegment(Segment(Joint(Joint.TransZ), - Frame(Vector(0.0,0.0,1.4)))) + Frame(Vector(0.0, 0.0, 1.4)))) self.chain.addSegment(Segment(Joint(Joint.TransX), - Frame(Vector(0.0,0.0,0.0)))) + Frame(Vector(0.0, 0.0, 0.0)))) self.chain.addSegment(Segment(Joint(Joint.TransY), - Frame(Vector(0.0,0.0,0.4)))) - self.chain.addSegment(Segment(Joint(Joint.None), - Frame(Vector(0.0,0.0,0.0)))) + Frame(Vector(0.0, 0.0, 0.4)))) + self.chain.addSegment(Segment(Joint(Joint.Fixed), + Frame(Vector(0.0, 0.0, 0.0)))) - self.jacsolver = ChainJntToJacSolver(self.chain) + self.jacsolver = ChainJntToJacSolver(self.chain) + self.jacdotsolver = ChainJntToJacDotSolver(self.chain) self.fksolverpos = ChainFkSolverPos_recursive(self.chain) self.fksolvervel = ChainFkSolverVel_recursive(self.chain) self.iksolvervel = ChainIkSolverVel_pinv(self.chain) - self.iksolverpos = ChainIkSolverPos_NR(self.chain,self.fksolverpos,self.iksolvervel) + self.iksolvervel_givens = ChainIkSolverVel_pinv_givens(self.chain) + self.iksolverpos = ChainIkSolverPos_NR(self.chain, self.fksolverpos, self.iksolvervel) + self.iksolverpos_givens = ChainIkSolverPos_NR(self.chain, self.fksolverpos, self.iksolvervel_givens) + + def testRotationalInertia(self): + ri = RotationalInertia(1, 2, 3, 4, 7, 5) + # __getitem__ + for i in range(3): + for j in range(3): + self.assertEqual(ri[3*i+j], 2*abs(i-j) + max(i, j) + 1) + with self.assertRaises(IndexError): + _ = ri[-1] + with self.assertRaises(IndexError): + _ = ri[9] + + # __setitem__ + for i in range(3): + for j in range(3): + ri[i] = i + for i in range(3): + for j in range(3): + self.assertEqual(ri[i], i) + with self.assertRaises(IndexError): + ri[-1] = 1 + with self.assertRaises(IndexError): + ri[9] = 1 + + def testJacobian(self): + jac = Jacobian(3) + for i in range(jac.columns()): + jac.setColumn(i, Twist(Vector(6*i+1, 6*i+2, 6*i+3), Vector(6*i+4, 6*i+5, 6*i+6))) + # __getitem__ + for i in range(6): + for j in range(3): + self.assertEqual(jac[i, j], 6*j+i+1) + with self.assertRaises(IndexError): + _ = jac[-1, 0] + with self.assertRaises(IndexError): + _ = jac[6, 0] + with self.assertRaises(IndexError): + _ = jac[5, -1] + with self.assertRaises(IndexError): + _ = jac[5, 3] + + # __setitem__ + for i in range(6): + for j in range(3): + jac[i, j] = 3*i+j + for i in range(6): + for j in range(3): + self.assertEqual(jac[i, j], 3*i+j) + with self.assertRaises(IndexError): + jac[-1, 0] = 1 + with self.assertRaises(IndexError): + jac[6, 0] = 1 + with self.assertRaises(IndexError): + jac[5, -1] = 1 + with self.assertRaises(IndexError): + jac[5, 3] = 1 + + def testJntArray(self): + ja = JntArray(3) + # __getitem__ + for i in range(3): + self.assertEqual(ja[i], 0) + with self.assertRaises(IndexError): + _ = ja[-1] + with self.assertRaises(IndexError): + _ = ja[3] + + # __setitem__ + for i in range(3): + ja[i] = 2*i + for i in range(3): + self.assertEqual(ja[i], 2*i) + with self.assertRaises(IndexError): + ja[-1] = 1 + with self.assertRaises(IndexError): + ja[3] = 1 def testFkPosAndJac(self): deltaq = 1E-4 epsJ = 1E-4 - F1=Frame() - F2=Frame() + F1 = Frame() + F2 = Frame() + + q = JntArray(self.chain.getNrOfJoints()) + jac = Jacobian(self.chain.getNrOfJoints()) - q=JntArray(self.chain.getNrOfJoints()) - jac=Jacobian(self.chain.getNrOfJoints()) - for i in range(q.rows()): - q[i]=random.uniform(-3.14,3.14) + q[i] = random.uniform(-3.14, 3.14) + + self.jacsolver.JntToJac(q, jac) - self.jacsolver.JntToJac(q,jac) - for i in range(q.rows()): - oldqi=q[i]; - q[i]=oldqi+deltaq - self.assertTrue(0==self.fksolverpos.JntToCart(q,F2)) - q[i]=oldqi-deltaq - self.assertTrue(0==self.fksolverpos.JntToCart(q,F1)) - q[i]=oldqi - Jcol1 = diff(F1,F2,2*deltaq) - Jcol2 = Twist(Vector(jac[0,i],jac[1,i],jac[2,i]), - Vector(jac[3,i],jac[4,i],jac[5,i])) - self.assertEqual(Jcol1,Jcol2); + oldqi = q[i] + q[i] = oldqi+deltaq + self.assertTrue(0 == self.fksolverpos.JntToCart(q, F2)) + q[i] = oldqi-deltaq + self.assertTrue(0 == self.fksolverpos.JntToCart(q, F1)) + q[i] = oldqi + Jcol1 = diff(F1, F2, 2*deltaq) + Jcol2 = Twist(Vector(jac[0, i], jac[1, i], jac[2, i]), + Vector(jac[3, i], jac[4, i], jac[5, i])) + self.assertEqual(Jcol1, Jcol2) def testFkVelAndJac(self): deltaq = 1E-4 - epsJ = 1E-4 - - q=JntArray(self.chain.getNrOfJoints()) - qdot=JntArray(self.chain.getNrOfJoints()) + epsJ = 1E-4 + + q = JntArray(self.chain.getNrOfJoints()) + qdot = JntArray(self.chain.getNrOfJoints()) + for i in range(q.rows()): + q[i] = random.uniform(-3.14, 3.14) + qdot[i] = random.uniform(-3.14, 3.14) + + qvel = JntArrayVel(q, qdot) + jac = Jacobian(self.chain.getNrOfJoints()) + + cart = FrameVel.Identity() + t = Twist.Zero() + + self.jacsolver.JntToJac(qvel.q, jac) + self.assertTrue(self.fksolvervel.JntToCart(qvel, cart) == 0) + MultiplyJacobian(jac, qvel.qdot, t) + self.assertEqual(cart.deriv(), t) + + def testFkVelAndIkVelImpl(self, fksolvervel, iksolvervel, epsJ): + q = JntArray(self.chain.getNrOfJoints()) + qdot = JntArray(self.chain.getNrOfJoints()) for i in range(q.rows()): - q[i]=random.uniform(-3.14,3.14) - qdot[i]=random.uniform(-3.14,3.14) + q[i] = random.uniform(-3.14, 3.14) + qdot[i] = random.uniform(-3.14, 3.14) + + qvel = JntArrayVel(q, qdot) + qdot_solved = JntArray(self.chain.getNrOfJoints()) - qvel=JntArrayVel(q,qdot); - jac=Jacobian(self.chain.getNrOfJoints()) + cart = FrameVel() - cart = FrameVel.Identity(); - t = Twist.Zero(); + self.assertTrue(0 == fksolvervel.JntToCart(qvel, cart)) + self.assertTrue(0 == iksolvervel.CartToJnt(qvel.q, cart.deriv(), qdot_solved)) - self.jacsolver.JntToJac(qvel.q,jac) - self.assertTrue(self.fksolvervel.JntToCart(qvel,cart)==0) - MultiplyJacobian(jac,qvel.qdot,t) - self.assertEqual(cart.deriv(),t) + self.assertEqual(qvel.qdot, qdot_solved) def testFkVelAndIkVel(self): - epsJ = 1E-7 + epsJ = 1e-7 + self.testFkVelAndIkVelImpl(self.fksolvervel, self.iksolvervel, epsJ) + + def testFkVelAndIkVelGivens(self): + epsJ = 1e-7 + self.testFkVelAndIkVelImpl(self.fksolvervel, self.iksolvervel_givens, epsJ) - q=JntArray(self.chain.getNrOfJoints()) - qdot=JntArray(self.chain.getNrOfJoints()) + def testFkPosAndIkPosImpl(self, fksolverpos, iksolverpos, epsJ): + q = JntArray(self.chain.getNrOfJoints()) for i in range(q.rows()): - q[i]=random.uniform(-3.14,3.14) - qdot[i]=random.uniform(-3.14,3.14) + q[i] = random.uniform(-3.14, 3.14) - qvel=JntArrayVel(q,qdot) - qdot_solved=JntArray(self.chain.getNrOfJoints()) - - cart = FrameVel() - - self.assertTrue(0==self.fksolvervel.JntToCart(qvel,cart)) - self.assertTrue(0==self.iksolvervel.CartToJnt(qvel.q,cart.deriv(),qdot_solved)) - - self.assertEqual(qvel.qdot,qdot_solved); - + q_init = JntArray(self.chain.getNrOfJoints()) + for i in range(q_init.rows()): + q_init[i] = q[i]+0.1*random.random() + + q_solved = JntArray(q.rows()) + + F1 = Frame.Identity() + F2 = Frame.Identity() + + self.assertTrue(0 == fksolverpos.JntToCart(q, F1)) + self.assertTrue(0 == iksolverpos.CartToJnt(q_init, F1, q_solved)) + self.assertTrue(0 == fksolverpos.JntToCart(q_solved, F2)) + + self.assertEqual(F1, F2) + self.assertTrue(Equal(q, q_solved, epsJ), "{} != {}".format(q, q_solved)) def testFkPosAndIkPos(self): - q=JntArray(self.chain.getNrOfJoints()) + epsJ = 1e-3 + self.testFkPosAndIkPosImpl(self.fksolverpos, self.iksolverpos, epsJ) + + def testFkPosAndIkPosGivens(self): + epsJ = 1e-3 + self.testFkPosAndIkPosImpl(self.fksolverpos, self.iksolverpos_givens, epsJ) + + def compare_Jdot_Diff_vs_Solver(self, dt, representation): + NrOfJoints = self.chain.getNrOfJoints() + q = JntArray(NrOfJoints) + qdot = JntArray(NrOfJoints) for i in range(q.rows()): - q[i]=random.uniform(-3.14,3.14) - - q_init=JntArray(self.chain.getNrOfJoints()) - for i in range(q_init.rows()): - q_init[i]=q[i]+0.1*random.random() - - q_solved=JntArray(q.rows()) - - F1=Frame.Identity() - F2=Frame.Identity() - - self.assertTrue(0==self.fksolverpos.JntToCart(q,F1)) - self.assertTrue(0==self.iksolverpos.CartToJnt(q_init,F1,q_solved)) - self.assertTrue(0==self.fksolverpos.JntToCart(q_solved,F2)) - - self.assertEqual(F1,F2) - self.assertEqual(q,q_solved) - - + q[i] = random.random() + qdot[i] = random.random() + + q_dqdt = JntArray(q) + for i in range(q.rows()): + q_dqdt[i] += dt * qdot[i] + + F_bs_ee_q = Frame.Identity() + F_bs_ee_q_dqdt = Frame.Identity() + + jac_q = Jacobian(NrOfJoints) + jac_q_dqdt = Jacobian(NrOfJoints) + jdot_by_diff = Jacobian(NrOfJoints) + + self.jacsolver.JntToJac(q, jac_q) + self.jacsolver.JntToJac(q_dqdt, jac_q_dqdt) + + self.fksolverpos.JntToCart(q, F_bs_ee_q) + self.fksolverpos.JntToCart(q_dqdt, F_bs_ee_q_dqdt) + + changeJacRepresentation(jac_q, F_bs_ee_q, representation) + changeJacRepresentation(jac_q_dqdt, F_bs_ee_q_dqdt, representation) + + Jdot_diff(jac_q, jac_q_dqdt, dt, jdot_by_diff) + + jdot_by_solver = Jacobian(NrOfJoints) + self.jacdotsolver.setRepresentation(representation) + self.jacdotsolver.JntToJacDot(JntArrayVel(q_dqdt, qdot), jdot_by_solver) + + jdot_qdot_by_solver = Twist() + MultiplyJacobian(jdot_by_solver, qdot, jdot_qdot_by_solver) + + jdot_qdot_by_diff = Twist() + MultiplyJacobian(jdot_by_diff, qdot, jdot_qdot_by_diff) + + err = (jdot_qdot_by_diff.vel.Norm() - jdot_qdot_by_solver.vel.Norm() + + jdot_qdot_by_diff.rot.Norm() - jdot_qdot_by_solver.rot.Norm()) + return abs(err), jdot_qdot_by_solver, jdot_qdot_by_diff + + def testJacDot(self): + dt = 1e-6 + success = True + while dt <= 0.1: + eps_diff_vs_solver = 4*dt + representations = [ChainJntToJacDotSolver.HYBRID, + ChainJntToJacDotSolver.INERTIAL, + ChainJntToJacDotSolver.BODYFIXED] + + for representation in representations: + err, jdot_qdot_solver, jdot_qdot_diff = self.compare_Jdot_Diff_vs_Solver(dt, representation) + success &= err <= eps_diff_vs_solver + self.assertTrue(success, "{} != {}\n" + "representation: {}\n" + "dt: {}\n" + "eps_diff_vs_solver: {}\n" + "err: {}".format(jdot_qdot_solver, jdot_qdot_diff, representation, dt, + eps_diff_vs_solver, err)) + dt *= 10 + + +class KinfamTestTree(unittest.TestCase): + + def setUp(self): + self.tree = Tree() + self.tree.addSegment(Segment(Joint(Joint.RotZ), + Frame(Vector(0.0, 0.0, 0.0))), "foo") + self.tree.addSegment(Segment(Joint(Joint.Fixed), + Frame(Vector(0.0, 0.0, 0.0))), "bar") + + def testTreeGetChainMemLeak(self): + # test for the memory leak in Tree.getChain described in issue #211 + process = psutil.Process() + self.tree.getChain("foo", "bar") + gc.collect() + mem_before = process.memory_info().vms + # needs at least 2000 iterations on my system to cause a detectable + # difference in memory usage + for _ in range(10000): + self.tree.getChain("foo", "bar") + gc.collect() + mem_after = process.memory_info().vms + self.assertEqual(mem_before, mem_after) + + def suite(): suite = unittest.TestSuite() + suite.addTest(KinfamTestFunctions('testRotationalInertia')) + suite.addTest(KinfamTestFunctions('testJacobian')) + suite.addTest(KinfamTestFunctions('testJntArray')) suite.addTest(KinfamTestFunctions('testFkPosAndJac')) suite.addTest(KinfamTestFunctions('testFkVelAndJac')) suite.addTest(KinfamTestFunctions('testFkVelAndIkVel')) + suite.addTest(KinfamTestFunctions('testFkVelAndIkVelGivens')) suite.addTest(KinfamTestFunctions('testFkPosAndIkPos')) + suite.addTest(KinfamTestFunctions('testFkPosAndIkPosGivens')) + suite.addTest(KinfamTestFunctions('testJacDot')) + suite.addTest(KinfamTestTree('testTreeGetChainMemLeak')) return suite -#suite = suite() -#unittest.TextTestRunner(verbosity=3).run(suite) - + +if __name__ == '__main__': + import sys + suite = suite() + result = unittest.TextTestRunner(verbosity=3).run(suite) + + if result.wasSuccessful(): + sys.exit(0) + else: + sys.exit(1)