├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake ├── FindEigen3.cmake ├── FindGTest.cmake └── FindROS.cmake ├── cmake_uninstall.cmake.in ├── doc ├── cheatsheet │ ├── IEEEtran.bst │ ├── IEEEtran.cls │ ├── cheatsheet_kindr1.tex │ ├── cheatsheet_latest.pdf │ ├── coordinate_systems │ │ ├── coordinate_system_rpy_kindr1-crop.pdf │ │ ├── coordinate_system_rpy_kindr1.tex │ │ ├── coordinate_system_ypr_kindr1-crop.pdf │ │ ├── coordinate_system_ypr_kindr1.tex │ │ ├── rotation_active_passive-crop.pdf │ │ ├── rotation_active_passive.pdf │ │ ├── rotation_active_passive.tex │ │ └── tikz-3dplot.sty │ ├── pics │ │ └── rpy.png │ ├── references.bib │ └── styles │ │ └── def.tex ├── doxygen │ ├── CMakeLists.txt │ ├── doxygen.config.in │ ├── figures │ │ ├── EulerAnglesXyz.png │ │ ├── EulerAnglesZyx.png │ │ └── rotation_active_passive.png │ ├── mainpage.dox │ ├── page_pose.dox │ ├── page_posediff.dox │ ├── page_quaternion.dox │ ├── page_rotation.dox │ ├── page_rotationdiff.dox │ └── page_vector.dox └── notes_kindr_1.0.0.md ├── gtesting.txt ├── include └── kindr │ ├── Core │ ├── common │ ├── assert_macros.hpp │ ├── assert_macros_eigen.hpp │ ├── common.hpp │ ├── gtest_eigen.hpp │ └── source_file_pos.hpp │ ├── math │ └── LinearAlgebra.hpp │ ├── phys_quant │ ├── PhysicalQuantities.hpp │ ├── PhysicalType.hpp │ ├── Wrench.hpp │ └── WrenchBase.hpp │ ├── poses │ ├── HomogeneousTransformation.hpp │ ├── Pose.hpp │ ├── PoseBase.hpp │ ├── PoseDiff.hpp │ ├── PoseDiffBase.hpp │ └── Twist.hpp │ ├── quaternions │ ├── Quaternion.hpp │ └── QuaternionBase.hpp │ ├── rotations │ ├── AngleAxis.hpp │ ├── EulerAnglesXyz.hpp │ ├── EulerAnglesXyzDiff.hpp │ ├── EulerAnglesZyx.hpp │ ├── EulerAnglesZyxDiff.hpp │ ├── GlobalAngularVelocity.hpp │ ├── LocalAngularVelocity.hpp │ ├── Rotation.hpp │ ├── RotationBase.hpp │ ├── RotationConversion.hpp │ ├── RotationDiff.hpp │ ├── RotationDiffBase.hpp │ ├── RotationImpl.hpp │ ├── RotationMatrix.hpp │ ├── RotationMatrixDiff.hpp │ ├── RotationQuaternion.hpp │ ├── RotationQuaternionDiff.hpp │ ├── RotationVector.hpp │ └── gtest_rotations.hpp │ └── vectors │ ├── Vector.hpp │ └── VectorBase.hpp ├── kindrConfig.cmake.in ├── matlab ├── core │ ├── boxMinus.m │ ├── boxPlus.m │ ├── getAngleAxisFromRotationMatrix.m │ ├── getEulAngXYZFromRotationMatrix.m │ ├── getEulAngZXZFromRotationMatrix.m │ ├── getEulAngZYXFromRotationMatrix.m │ ├── getEulAngZYZFromRotationMatrix.m │ ├── getMapEulAngZXZDiffToAngVelInWorldFrame.m │ ├── getMapEulAngZYXDDiffToAngAccInBaseFrame.m │ ├── getMapEulAngZYXDDiffToAngAccInWorldFrame.m │ ├── getMapEulAngZYXDiffToAngVelInBaseFrame.m │ ├── getMapEulAngZYXDiffToAngVelInWorldFrame.m │ ├── getMapEulAngZYZDiffToAngVelInWorldFrame.m │ ├── getRotationMatrixX.m │ ├── getRotationMatrixY.m │ ├── getRotationMatrixZ.m │ ├── getTimeDerivative.m │ ├── isRotationMatrix.m │ ├── mapAngleAxisToQuaternion.m │ ├── mapEulerAnglesXYZToRotationMatrix.m │ ├── mapEulerAnglesZXZToRotationMatrix.m │ ├── mapEulerAnglesZYXToRotationMatrix.m │ ├── mapEulerAnglesZYZToRotationMatrix.m │ ├── mapQuaternionToRotationMatrix.m │ ├── mapRotationMatrixToQuaternion.m │ ├── mapRotationMatrixToRotationVector.m │ ├── mapRotationQuaternionToRotationVector.m │ ├── mapRotationVectorToRotationMatrix.m │ ├── rotateVectorUsingQuaternion.m │ ├── skew.m │ └── unskew.m ├── finite_differences │ ├── linearizationExample.m │ └── numerical_jacobian.m ├── utils │ ├── dMATdt.m │ ├── fulldMATdt.m │ ├── fulldiff2.m │ └── rotations.m └── visualization │ ├── addLabelToVector.m │ ├── animateAngularVelocityUsingEulerZYX.m │ ├── animateAngularVelocityUsingExpMap.m │ ├── animateLinearVelocity.m │ ├── visualizeCoordinateSystem.m │ ├── visualizeHomogeneousTransform.m │ ├── visualizeLinearization.m │ ├── visualizeRotationFromAngleAxis.m │ └── visualizeRotationFromRotationVector.m ├── package.xml ├── signature_of_kindr1_library └── test ├── CMakeLists.txt ├── common └── CommonTest.cpp ├── linear_algebra ├── PseudoInverseTest.cpp └── SkewMatrixFromVectorTest.cpp ├── phys_quant ├── ForceTest.cpp ├── ScalarTest.cpp └── WrenchTest.cpp ├── poses ├── HomogeneousTransformationTest.cpp ├── PoseDiffTest.cpp ├── PositionDiffTest.cpp ├── PositionTest.cpp └── TwistWithAngularVelocityTest.cpp ├── quaternions └── QuaternionTest.cpp ├── rotations ├── AngleAxisTest.cpp ├── ConventionTest.cpp ├── EulerAnglesXyzDiffTest.cpp ├── EulerAnglesXyzTest.cpp ├── EulerAnglesZyxDiffTest.cpp ├── EulerAnglesZyxTest.cpp ├── GlobalAngularVelocityTest.cpp ├── LocalAngularVelocityTest.cpp ├── RotationDiffTest.cpp ├── RotationMatrixDiffTest.cpp ├── RotationMatrixTest.cpp ├── RotationQuaternionDiffTest.cpp ├── RotationQuaternionTest.cpp ├── RotationTest.cpp └── RotationVectorTest.cpp ├── run_tests.py ├── test_main.cpp └── vectors └── VectorsTest.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Make 2 | CMakeCache.txt 3 | CMakeFiles 4 | Makefile 5 | cmake_install.cmake 6 | install_manifest.txt 7 | 8 | # Compiled Object files 9 | *.slo 10 | *.lo 11 | *.o 12 | 13 | # Compiled Dynamic libraries 14 | *.so 15 | *.dylib 16 | 17 | # Compiled Static libraries 18 | *.lai 19 | *.la 20 | *.a 21 | 22 | *~ 23 | 24 | 25 | # Eclipse 26 | .project 27 | .cproject 28 | .settings 29 | 30 | build/* 31 | gtest 32 | googletest-release-1.7.0 33 | release-1.7.0.zip 34 | 35 | # Latex 36 | *.acn 37 | *.acr 38 | *.alg 39 | *.aux 40 | *.bbl 41 | *.blg 42 | *.dvi 43 | *.fdb_latexmk 44 | *.glg 45 | *.glo 46 | *.gls 47 | *.idx 48 | *.ilg 49 | *.ind 50 | *.ist 51 | *.lof 52 | *.log 53 | *.lot 54 | *.maf 55 | *.mtc 56 | *.mtc0 57 | *.nav 58 | *.nlo 59 | *.out 60 | *.pdfsync 61 | *.ps 62 | *.snm 63 | *.synctex.gz 64 | *.toc 65 | *.vrb 66 | *.xdy 67 | *.tdo 68 | *-converted-to.pdf 69 | *~ 70 | 71 | cmake/FindKindr.cmake 72 | doc/doxygen.config 73 | doc/html 74 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # * Redistributions of source code must retain the above copyright 7 | # notice, this list of conditions and the following disclaimer. 8 | # * Redistributions in binary form must reproduce the above copyright 9 | # notice, this list of conditions and the following disclaimer in the 10 | # documentation and/or other materials provided with the distribution. 11 | # * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 12 | # names of its contributors may be used to endorse or promote products 13 | # derived from this software without specific prior written permission. 14 | # 15 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | # DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 19 | # Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 20 | # OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 21 | # GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 22 | # HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 23 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | cmake_minimum_required(VERSION 3.16.3) 27 | project(kindr) 28 | 29 | 30 | 31 | if("$ENV{ROS_VERSION}" STREQUAL "1" OR USE_CMAKE) 32 | 33 | 34 | 35 | # Check if catkin package should be built. 36 | if(NOT USE_CMAKE) 37 | find_package(catkin QUIET) 38 | message(STATUS "Setting pure CMake build to false as not specified.") 39 | set(USE_CMAKE false CACHE BOOL "Choose whether to build with CMake." FORCE) 40 | set_property(CACHE USE_CMAKE PROPERTY STRINGS "True" "False") 41 | endif() 42 | 43 | if(catkin_FOUND AND NOT USE_CMAKE) 44 | message(STATUS "Building kindr as a catkin package.") 45 | set(BUILD_CATKIN_PACKAGE true) 46 | set(BUILD_TEST false) 47 | else() 48 | message(STATUS "Building kindr as a cmake package.") 49 | set(BUILD_CATKIN_PACKAGE false) 50 | endif() 51 | 52 | # Configure CMake. 53 | if(NOT BUILD_CATKIN_PACKAGE) 54 | set(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}) 55 | set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH}) 56 | endif() 57 | 58 | # Noisily default to Release build 59 | if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) 60 | message(STATUS "Setting build type to 'Release' as none was specified.") 61 | set(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build." FORCE) 62 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS 63 | "Debug" "Release" "MinSizeRel" "RelWithDebInfo") 64 | endif() 65 | 66 | add_definitions(-std=c++11) 67 | 68 | find_package(Eigen3 REQUIRED) 69 | 70 | # Init catkin package. 71 | if(BUILD_CATKIN_PACKAGE) 72 | catkin_package( 73 | INCLUDE_DIRS 74 | include 75 | ${EIGEN3_INCLUDE_DIR} 76 | ) 77 | endif() 78 | 79 | include_directories(include) 80 | include_directories(${EIGEN3_INCLUDE_DIR}) 81 | 82 | # Don't build tests if not specified. 83 | if(NOT BUILD_TEST) 84 | message(STATUS "Setting build-tests to false as not specified.") 85 | set(BUILD_TEST false CACHE BOOL "Choose whether to build tests." FORCE) 86 | set_property(CACHE BUILD_TEST PROPERTY STRINGS 87 | "True" "False") 88 | endif() 89 | 90 | if(BUILD_TEST AND EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/gtest/") 91 | message(STATUS "Building GTests!") 92 | option(BUILD_GTEST "build gtest" ON) 93 | add_subdirectory(gtest gtest) 94 | enable_testing() 95 | endif() 96 | 97 | install(DIRECTORY include/kindr DESTINATION include) 98 | 99 | # Add unit tests 100 | add_subdirectory(test) 101 | 102 | # Add Doxygen documentation 103 | add_subdirectory(doc/doxygen) 104 | 105 | # Install catkin package.xml 106 | #install(FILES package.xml DESTINATION share/kindr) 107 | 108 | 109 | #============================================= 110 | # to allow find_package() on kindr 111 | #============================================= 112 | # 113 | # the following case be used in an external project requiring kindr: 114 | # ... 115 | # find_package(kindr) 116 | # include_directories(${kindr_INCLUDE_DIRS}) 117 | # ... 118 | 119 | # NOTE: the following will support find_package for 1) local build (make) and 2) for installed files (make install) 120 | 121 | # 1- local build # 122 | 123 | # Register the local build in case one doesn't use "make install" 124 | export(PACKAGE kindr) 125 | 126 | # Create variable for the local build tree 127 | get_property(kindr_include_dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES) 128 | 129 | # Configure config file for local build tree 130 | configure_file(kindrConfig.cmake.in 131 | "${PROJECT_BINARY_DIR}/kindrConfig.cmake" @ONLY) 132 | 133 | # 2- installation build # 134 | 135 | # Change the include location for the case of an install location 136 | set(kindr_include_dirs ${CMAKE_INSTALL_PREFIX}/include ${EIGEN3_INCLUDE_DIR}) 137 | 138 | 139 | # We put the generated file for installation in a different repository (i.e., ./CMakeFiles/) 140 | configure_file(kindrConfig.cmake.in 141 | "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/kindrConfig.cmake" @ONLY) 142 | 143 | install(FILES 144 | "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/kindrConfig.cmake" 145 | DESTINATION share/kindr/cmake COMPONENT dev) 146 | 147 | install(FILES 148 | package.xml 149 | DESTINATION share/kindr COMPONENT dev) 150 | 151 | 152 | #============================================= 153 | # Add uninstall target 154 | #============================================= 155 | configure_file( 156 | "${CMAKE_CURRENT_SOURCE_DIR}/cmake_uninstall.cmake.in" 157 | "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake" 158 | IMMEDIATE @ONLY) 159 | 160 | if (NOT TARGET uninstall) 161 | add_custom_target(uninstall 162 | COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake) 163 | endif() 164 | 165 | 166 | 167 | else() # ROS version 2 168 | 169 | 170 | set(CMAKE_CXX_STANDARD 14) 171 | 172 | find_package(ament_cmake REQUIRED) 173 | find_package(Eigen3 REQUIRED) 174 | 175 | add_library( 176 | ${PROJECT_NAME} 177 | INTERFACE 178 | ) 179 | target_include_directories( 180 | ${PROJECT_NAME} 181 | INTERFACE 182 | "$" "$" 183 | ${EIGEN3_INCLUDE_DIR} 184 | ) 185 | ament_target_dependencies(${PROJECT_NAME} Eigen3) 186 | 187 | # Add Doxygen documentation 188 | add_subdirectory(doc/doxygen) 189 | 190 | install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) 191 | install( 192 | TARGETS ${PROJECT_NAME} 193 | EXPORT export_${PROJECT_NAME} 194 | LIBRARY DESTINATION lib 195 | ARCHIVE DESTINATION lib 196 | RUNTIME DESTINATION bin 197 | ) 198 | 199 | if(BUILD_TESTING) 200 | add_subdirectory(test) 201 | endif() 202 | 203 | ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) 204 | ament_export_dependencies(Eigen3) 205 | 206 | ament_package() 207 | 208 | endif() 209 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2019, ANYbotics AG. 2 | 3 | Redistribution and use in source and binary forms, with or without 4 | modification, are permitted provided that the following conditions 5 | are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | 10 | 2. Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in 12 | the documentation and/or other materials provided with the 13 | distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived 17 | from this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 22 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 23 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 26 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 27 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Kindr - Kinematics and Dynamics for Robotics 2 | ============================================= 3 | 4 | **Authors: Christian Gehring, C. Dario Bellicoso, Michael Bloesch, Remo Diethelm, Peter Fankhauser, Paul Furgale, Michael Neunert, Hannes Sommer
5 | Maintainer: Remo Diethelm, rdiethelm@anybotics.com
6 | Affiliation: [ANYbotics](https://www.anybotics.com/)** 7 | 8 | This projected was initially developed at ETH Zurich (Autonomous Systems Lab & Robotic Systems Lab). 9 | 10 | This work is conducted as part of [ANYmal Research](https://www.anymal-research.org/), a community to advance legged robotics. 11 | 12 | The source code is released under a [BSD 3-Clause license](LICENSE). 13 | 14 | ## Documentation 15 | 16 | [Online](https://opensource.docs.anymal.com/doxygen/kindr/master/index.html) 17 | 18 | Impatient individuals can directly download the [cheat sheet](https://opensource.docs.anymal.com/doxygen/kindr/master/cheatsheet_latest.pdf). 19 | 20 | See also section 'Building the documentation' below. 21 | 22 | ## Changelog 23 | 24 | ### Kindr 1.0.0 25 | 26 | * Simplified header include `#include ` is provided. 27 | * Kindr is now strongly dependent on Eigen. 28 | * All sub namespaces have been removed. (e.g. `kindr::rotations::eigen_impl` -> `kindr::`) 29 | * The implementations of rotations and time derivatives have been simplified (Passive, Hamiltonian). 30 | - Active typedefs (e.g. RotationQuaternionAD) have been removed and simpler ones (e.g. RotationQuaternionD) have been introduced. 31 | - Note that the functionality of some operators changed! Please check the [cheat sheet](https://opensource.docs.anymal.com/doxygen/kindr/master/cheatsheet_latest.pdf) to understand what is implemented. 32 | - Some hints on what needs to be changed from kindr 0.0.1: 33 | - `rotation.setFromVectors(v1, v2)` -> `rotation.setFromVectors(v2, v1)` 34 | - `C_BI.boxPlus(dt * B_w_IB)` -> `C_BI.boxPlus(dt * C_IB * B_w_IB)` 35 | - `C_BI.boxMinus(dt * B_w_IB)` -> `-C_BI.boxMinus(dt * B_w_IB)` 36 | - Euler angles probably have to be negated. 37 | * Conversion methods between ROS and kindr have been moved to the package [kindr_ros](https://github.com/anybotics/kindr_ros). 38 | * Concatenation of Homogeneous Transformation is now implemented. 39 | * Short typedefs are provided for Homogeneous Transformation: `HomTransformQuatD`, `HomTransformMatrixD`. 40 | * Jacobian of exponential map is implemented. 41 | * Unit tests based on gtest are provided to test the convention of other software packages. 42 | - Gazebo (gazebo::math::Quaternion) uses the same convention as kindr. 43 | - ROS TF (tf::Quaternion and tf::Matrix3x3) uses the same convention as kindr. 44 | - RBDL's RigidBodyDynamics::Math::SpatialTransform uses the same convention as kindr, whereas RBDL's RigidBodyDynamics::Math::Quaternion concatenates differently and its conversion to a rotation matrix is inverted. 45 | 46 | 47 | ## Requirements 48 | 49 | * [Eigen 3.2.0](http://eigen.tuxfamily.org) (Older versions might also work) 50 | * GCC 4.7 is required at the minimum. 51 | * CMake 2.8.3 is required at the minimum. 52 | 53 | ## Building 54 | 55 | ### Building with cmake 56 | 57 | Install the library with [CMake](www.cmake.org): 58 | 59 | ```bash 60 | mkdir build 61 | cd build 62 | cmake .. -DUSE_CMAKE=true 63 | sudo make install 64 | ``` 65 | 66 | Note that `USE_CMAKE` defaults to `true` if catkin is not installed. 67 | 68 | Uninstall the library with: 69 | 70 | ```bash 71 | cd build 72 | sudo make uninstall 73 | ``` 74 | 75 | Kindr can be included in your cmake project. 76 | Add the following to your *CmakeLists.txt*: 77 | 78 | ``` 79 | find_package(kindr) 80 | include_directories(${kindr_INCLUDE_DIRS}) 81 | ``` 82 | 83 | ### Building with catkin 84 | 85 | Build kindr with [catkin](wiki.ros.org/catkin): 86 | 87 | ```bash 88 | cd ~/catkin_ws/src 89 | git clone git@github.com:anybotics/kindr.git 90 | catkin_make_isolated -C ~/catkin_ws 91 | ``` 92 | 93 | or with [catkin command line tools](http://catkin-tools.readthedocs.org): 94 | 95 | ```bash 96 | cd ~/catkin_ws/src 97 | git clone git@github.com:anybotics/kindr.git 98 | catkin build -w ~/catkin_ws kindr 99 | ``` 100 | 101 | Kindr can be included in your catkin project with: 102 | Add the following to your *CMakeLists.txt*: 103 | ``` 104 | find_package(catkin COMPONENTS kindr) 105 | include_directories(${catkin_INCLUDE_DIRS}) 106 | ``` 107 | 108 | And to your *package.xml*: 109 | 110 | ```xml 111 | 112 | kindr 113 | 114 | ``` 115 | 116 | 117 | ### Building the documentation 118 | 119 | Build the documentation with [Doxygen](https://www.doxygen.nl/): 120 | ```bash 121 | mkdir build 122 | cd build 123 | cmake .. 124 | make kindr_doc 125 | ``` 126 | 127 | The doxygen documentation can be found here: 128 | 129 | ``` 130 | build/doc/html/index.html 131 | ``` 132 | 133 | ### Building unit tests with gtest 134 | 135 | [GTests](https://code.google.com/p/googletest/) are only built if the folder *gtest* exists in the root folder. 136 | 137 | Download and use GTest: 138 | 139 | ```bash 140 | wget https://github.com/google/googletest/archive/release-1.7.0.zip 141 | unzip release-1.7.0.zip 142 | ln -s googletest-release-1.7.0 gtest 143 | mkdir build 144 | cd build 145 | cmake .. -DUSE_CMAKE=true -DBUILD_TEST=true 146 | make 147 | ``` 148 | 149 | Note that `USE_CMAKE` defaults to `true` if catkin is not installed. 150 | -------------------------------------------------------------------------------- /cmake/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN3_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN3_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 79 | 80 | endif(EIGEN3_INCLUDE_DIR) 81 | -------------------------------------------------------------------------------- /cmake/FindGTest.cmake: -------------------------------------------------------------------------------- 1 | # Locate the Google C++ Testing Framework. 2 | # 3 | # Defines the following variables: 4 | # 5 | # GTEST_FOUND - Found the Google Testing framework 6 | # GTEST_INCLUDE_DIRS - Include directories 7 | # 8 | # Also defines the library variables below as normal 9 | # variables. These contain debug/optimized keywords when 10 | # a debugging library is found. 11 | # 12 | # GTEST_BOTH_LIBRARIES - Both libgtest & libgtest-main 13 | # GTEST_LIBRARIES - libgtest 14 | # GTEST_MAIN_LIBRARIES - libgtest-main 15 | # 16 | # Accepts the following variables as input: 17 | # 18 | # GTEST_ROOT - (as a CMake or environment variable) 19 | # The root directory of the gtest install prefix 20 | # 21 | # GTEST_MSVC_SEARCH - If compiling with MSVC, this variable can be set to 22 | # "MD" or "MT" to enable searching a GTest build tree 23 | # (defaults: "MD") 24 | # 25 | #----------------------- 26 | # Example Usage: 27 | # 28 | # enable_testing() 29 | # find_package(GTest REQUIRED) 30 | # include_directories(${GTEST_INCLUDE_DIRS}) 31 | # 32 | # add_executable(foo foo.cc) 33 | # target_link_libraries(foo ${GTEST_BOTH_LIBRARIES}) 34 | # 35 | # add_test(AllTestsInFoo foo) 36 | # 37 | #----------------------- 38 | # 39 | # If you would like each Google test to show up in CTest as 40 | # a test you may use the following macro. 41 | # NOTE: It will slow down your tests by running an executable 42 | # for each test and test fixture. You will also have to rerun 43 | # CMake after adding or removing tests or test fixtures. 44 | # 45 | # GTEST_ADD_TESTS(executable extra_args ARGN) 46 | # executable = The path to the test executable 47 | # extra_args = Pass a list of extra arguments to be passed to 48 | # executable enclosed in quotes (or "" for none) 49 | # ARGN = A list of source files to search for tests & test 50 | # fixtures. 51 | # 52 | # Example: 53 | # set(FooTestArgs --foo 1 --bar 2) 54 | # add_executable(FooTest FooUnitTest.cc) 55 | # GTEST_ADD_TESTS(FooTest "${FooTestArgs}" FooUnitTest.cc) 56 | 57 | #============================================================================= 58 | # Copyright 2009 Kitware, Inc. 59 | # Copyright 2009 Philip Lowman 60 | # Copyright 2009 Daniel Blezek 61 | # 62 | # Distributed under the OSI-approved BSD License (the "License"); 63 | # see accompanying file Copyright.txt for details. 64 | # 65 | # This software is distributed WITHOUT ANY WARRANTY; without even the 66 | # implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 67 | # See the License for more information. 68 | #============================================================================= 69 | # (To distribute this file outside of CMake, substitute the full 70 | # License text for the above reference.) 71 | # 72 | # Thanks to Daniel Blezek for the GTEST_ADD_TESTS code 73 | 74 | function(GTEST_ADD_TESTS executable extra_args) 75 | if(NOT ARGN) 76 | message(FATAL_ERROR "Missing ARGN: Read the documentation for GTEST_ADD_TESTS") 77 | endif() 78 | foreach(source ${ARGN}) 79 | file(READ "${source}" contents) 80 | string(REGEX MATCHALL "TEST_?F?\\(([A-Za-z_0-9 ,]+)\\)" found_tests ${contents}) 81 | foreach(hit ${found_tests}) 82 | string(REGEX REPLACE ".*\\( *([A-Za-z_0-9]+), *([A-Za-z_0-9]+) *\\).*" "\\1.\\2" test_name ${hit}) 83 | add_test(${test_name} ${executable} --gtest_filter=${test_name} ${extra_args}) 84 | endforeach() 85 | endforeach() 86 | endfunction() 87 | 88 | function(_gtest_append_debugs _endvar _library) 89 | if(${_library} AND ${_library}_DEBUG) 90 | set(_output optimized ${${_library}} debug ${${_library}_DEBUG}) 91 | else() 92 | set(_output ${${_library}}) 93 | endif() 94 | set(${_endvar} ${_output} PARENT_SCOPE) 95 | endfunction() 96 | 97 | function(_gtest_find_library _name) 98 | find_library(${_name} 99 | NAMES ${ARGN} 100 | HINTS 101 | $ENV{GTEST_ROOT} 102 | ${GTEST_ROOT} 103 | PATH_SUFFIXES ${_gtest_libpath_suffixes} 104 | ) 105 | mark_as_advanced(${_name}) 106 | endfunction() 107 | 108 | # 109 | 110 | if(NOT DEFINED GTEST_MSVC_SEARCH) 111 | set(GTEST_MSVC_SEARCH MD) 112 | endif() 113 | 114 | set(_gtest_libpath_suffixes lib) 115 | if(MSVC) 116 | if(GTEST_MSVC_SEARCH STREQUAL "MD") 117 | list(APPEND _gtest_libpath_suffixes 118 | msvc/gtest-md/Debug 119 | msvc/gtest-md/Release) 120 | elseif(GTEST_MSVC_SEARCH STREQUAL "MT") 121 | list(APPEND _gtest_libpath_suffixes 122 | msvc/gtest/Debug 123 | msvc/gtest/Release) 124 | endif() 125 | endif() 126 | 127 | 128 | find_path(GTEST_INCLUDE_DIR gtest/gtest.h 129 | HINTS 130 | $ENV{GTEST_ROOT}/include 131 | ${GTEST_ROOT}/include 132 | ) 133 | mark_as_advanced(GTEST_INCLUDE_DIR) 134 | 135 | if(MSVC AND GTEST_MSVC_SEARCH STREQUAL "MD") 136 | # The provided /MD project files for Google Test add -md suffixes to the 137 | # library names. 138 | _gtest_find_library(GTEST_LIBRARY gtest-md gtest) 139 | _gtest_find_library(GTEST_LIBRARY_DEBUG gtest-mdd gtestd) 140 | _gtest_find_library(GTEST_MAIN_LIBRARY gtest_main-md gtest_main) 141 | _gtest_find_library(GTEST_MAIN_LIBRARY_DEBUG gtest_main-mdd gtest_maind) 142 | else() 143 | _gtest_find_library(GTEST_LIBRARY gtest) 144 | _gtest_find_library(GTEST_LIBRARY_DEBUG gtestd) 145 | _gtest_find_library(GTEST_MAIN_LIBRARY gtest_main) 146 | _gtest_find_library(GTEST_MAIN_LIBRARY_DEBUG gtest_maind) 147 | endif() 148 | 149 | include(${CMAKE_CURRENT_LIST_DIR}/FindPackageHandleStandardArgs.cmake) 150 | FIND_PACKAGE_HANDLE_STANDARD_ARGS(GTest DEFAULT_MSG GTEST_LIBRARY GTEST_INCLUDE_DIR GTEST_MAIN_LIBRARY) 151 | 152 | if(GTEST_FOUND) 153 | set(GTEST_INCLUDE_DIRS ${GTEST_INCLUDE_DIR}) 154 | _gtest_append_debugs(GTEST_LIBRARIES GTEST_LIBRARY) 155 | _gtest_append_debugs(GTEST_MAIN_LIBRARIES GTEST_MAIN_LIBRARY) 156 | set(GTEST_BOTH_LIBRARIES ${GTEST_LIBRARIES} ${GTEST_MAIN_LIBRARIES}) 157 | endif() 158 | 159 | -------------------------------------------------------------------------------- /cmake/FindROS.cmake: -------------------------------------------------------------------------------- 1 | # - Find ROS 2 | # This module finds an installed ROS package. 3 | # 4 | # It sets the following variables: 5 | # ROS_FOUND - Set to false, or undefined, if ROS isn't found. 6 | 7 | IF(UNIX) 8 | FIND_PROGRAM(ROS_EXEC NAME rosrun PATHS) 9 | IF(ROS_EXEC) 10 | SET(ROS_FOUND TRUE) 11 | ENDIF(ROS_EXEC) 12 | ENDIF(UNIX) 13 | 14 | IF (ROS_FOUND) 15 | # show which ROS was found only if not quiet 16 | IF (NOT ROS_FIND_QUIETLY) 17 | MESSAGE(STATUS "Found ROS") 18 | ENDIF (NOT ROS_FIND_QUIETLY) 19 | ELSE (ROS_FOUND) 20 | # warning if ROS is required but not found 21 | IF (ROS_FIND_REQUIRED) 22 | MESSAGE(WARNING "Could not find ROS") 23 | ENDIF (ROS_FIND_REQUIRED) 24 | ENDIF (ROS_FOUND) 25 | 26 | IF (ROS_FOUND) 27 | 28 | # Set node dependencies 29 | set(rospack_dep roscpp) 30 | set(rospack_cmd rospack) 31 | 32 | # Get ROS includes 33 | set(rospack_arg cflags-only-I ${rospack_dep}) 34 | execute_process( 35 | COMMAND ${rospack_cmd} ${rospack_arg} 36 | OUTPUT_VARIABLE rospack_inc_dir 37 | OUTPUT_STRIP_TRAILING_WHITESPACE) 38 | string(REPLACE " " ";" ROSPACK_INC_DIR ${rospack_inc_dir}) 39 | 40 | # Get library folders 41 | set(rospack_arg libs-only-L ${rospack_dep}) 42 | execute_process( 43 | COMMAND ${rospack_cmd} ${rospack_arg} 44 | OUTPUT_VARIABLE rospack_lib_dir 45 | OUTPUT_STRIP_TRAILING_WHITESPACE) 46 | string(REPLACE " " ";" ROSPACK_LIB_DIR ${rospack_lib_dir}) 47 | 48 | # Get libraries 49 | set(rospack_arg libs-only-l ${rospack_dep}) 50 | execute_process( 51 | COMMAND ${rospack_cmd} ${rospack_arg} 52 | OUTPUT_VARIABLE rospack_libs 53 | OUTPUT_STRIP_TRAILING_WHITESPACE) 54 | string(REPLACE " " ";" ROSPACK_LIBS ${rospack_libs}) 55 | 56 | # Include directories 57 | include_directories(${ROSPACK_INC_DIR}) 58 | 59 | # Linking directories 60 | link_directories(${ROSPACK_LIB_DIR}) 61 | 62 | ENDIF (ROS_FOUND) -------------------------------------------------------------------------------- /cmake_uninstall.cmake.in: -------------------------------------------------------------------------------- 1 | if(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") 2 | message(FATAL_ERROR "Cannot find install manifest: @CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") 3 | endif(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") 4 | 5 | file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files) 6 | string(REGEX REPLACE "\n" ";" files "${files}") 7 | foreach(file ${files}) 8 | message(STATUS "Uninstalling $ENV{DESTDIR}${file}") 9 | if(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}") 10 | exec_program( 11 | "@CMAKE_COMMAND@" ARGS "-E remove \"$ENV{DESTDIR}${file}\"" 12 | OUTPUT_VARIABLE rm_out 13 | RETURN_VALUE rm_retval 14 | ) 15 | if(NOT "${rm_retval}" STREQUAL 0) 16 | message(FATAL_ERROR "Problem when removing $ENV{DESTDIR}${file}") 17 | endif(NOT "${rm_retval}" STREQUAL 0) 18 | else(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}") 19 | message(STATUS "File $ENV{DESTDIR}${file} does not exist.") 20 | endif(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}") 21 | endforeach(file) 22 | 23 | 24 | -------------------------------------------------------------------------------- /doc/cheatsheet/cheatsheet_latest.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/kindr/32800890d546e306f73c3ee3091fb6903452f925/doc/cheatsheet/cheatsheet_latest.pdf -------------------------------------------------------------------------------- /doc/cheatsheet/coordinate_systems/coordinate_system_rpy_kindr1-crop.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/kindr/32800890d546e306f73c3ee3091fb6903452f925/doc/cheatsheet/coordinate_systems/coordinate_system_rpy_kindr1-crop.pdf -------------------------------------------------------------------------------- /doc/cheatsheet/coordinate_systems/coordinate_system_ypr_kindr1-crop.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/kindr/32800890d546e306f73c3ee3091fb6903452f925/doc/cheatsheet/coordinate_systems/coordinate_system_ypr_kindr1-crop.pdf -------------------------------------------------------------------------------- /doc/cheatsheet/coordinate_systems/rotation_active_passive-crop.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/kindr/32800890d546e306f73c3ee3091fb6903452f925/doc/cheatsheet/coordinate_systems/rotation_active_passive-crop.pdf -------------------------------------------------------------------------------- /doc/cheatsheet/coordinate_systems/rotation_active_passive.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/kindr/32800890d546e306f73c3ee3091fb6903452f925/doc/cheatsheet/coordinate_systems/rotation_active_passive.pdf -------------------------------------------------------------------------------- /doc/cheatsheet/coordinate_systems/rotation_active_passive.tex: -------------------------------------------------------------------------------- 1 | \documentclass[landscape]{article} 2 | \thispagestyle{empty} 3 | \usepackage{amsmath,amssymb} 4 | \usepackage{wasysym} 5 | \usepackage{tikz} 6 | \usetikzlibrary{plotmarks} 7 | \usepackage{tikz-3dplot} 8 | \usepackage{pgfplots} 9 | \usepackage[margin=0cm]{geometry} 10 | 11 | % Workaround for making use of externalization possible 12 | % -> remove hardcoded pdflatex and replace by lualatex 13 | % \usepgfplotslibrary{external} 14 | % \tikzset{external/system call={lualatex \tikzexternalcheckshellescape% 15 | % -halt-on-error -interaction=batchmode -jobname "\image" "\texsource"}} 16 | 17 | % Redefine rotation sequence for tikz3d-plot to z-y-x 18 | \newcommand{\tdseteulerxyz}{ 19 | \renewcommand{\tdplotcalctransformrotmain}{% 20 | %perform some trig for the Euler transformation 21 | \tdplotsinandcos{\sinalpha}{\cosalpha}{\tdplotalpha} 22 | \tdplotsinandcos{\sinbeta}{\cosbeta}{\tdplotbeta} 23 | \tdplotsinandcos{\singamma}{\cosgamma}{\tdplotgamma} 24 | % 25 | \tdplotmult{\sasb}{\sinalpha}{\sinbeta} 26 | \tdplotmult{\sasg}{\sinalpha}{\singamma} 27 | \tdplotmult{\sasbsg}{\sasb}{\singamma} 28 | % 29 | \tdplotmult{\sacb}{\sinalpha}{\cosbeta} 30 | \tdplotmult{\sacg}{\sinalpha}{\cosgamma} 31 | \tdplotmult{\sasbcg}{\sasb}{\cosgamma} 32 | % 33 | \tdplotmult{\casb}{\cosalpha}{\sinbeta} 34 | \tdplotmult{\cacb}{\cosalpha}{\cosbeta} 35 | \tdplotmult{\cacg}{\cosalpha}{\cosgamma} 36 | \tdplotmult{\casg}{\cosalpha}{\singamma} 37 | % 38 | \tdplotmult{\cbsg}{\cosbeta}{\singamma} 39 | \tdplotmult{\cbcg}{\cosbeta}{\cosgamma} 40 | % 41 | \tdplotmult{\casbsg}{\casb}{\singamma} 42 | \tdplotmult{\casbcg}{\casb}{\cosgamma} 43 | % 44 | %determine rotation matrix elements for Euler transformation 45 | \pgfmathsetmacro{\raaeul}{\cacb} 46 | \pgfmathsetmacro{\rabeul}{\casbsg - \sacg} 47 | \pgfmathsetmacro{\raceul}{\sasg + \casbcg} 48 | \pgfmathsetmacro{\rbaeul}{\sacb} 49 | \pgfmathsetmacro{\rbbeul}{\sasbsg + \cacg} 50 | \pgfmathsetmacro{\rbceul}{\sasbcg - \casg} 51 | \pgfmathsetmacro{\rcaeul}{-\sinbeta} 52 | \pgfmathsetmacro{\rcbeul}{\cbsg} 53 | \pgfmathsetmacro{\rcceul}{\cbcg} 54 | } 55 | } 56 | 57 | \tdseteulerxyz 58 | 59 | \usepackage{siunitx} 60 | \input{../styles/def} 61 | 62 | %%%%%%%%% Z 63 | \begin{document} 64 | \clearpage 65 | \centering 66 | 67 | 68 | 69 | % Set the plot display orientation 70 | % Syntax: \tdplotsetdisplay{\theta_d}{\phi_d} 71 | 72 | \tdplotsetmaincoords{60}{120} 73 | %\tdplotsetmaincoords{60}{140} 74 | 75 | \pgfmathsetmacro{\zRot}{20} 76 | \pgfmathsetmacro{\yRot}{25} 77 | \pgfmathsetmacro{\xRot}{30} 78 | 79 | 80 | %%%%%%%%%%%%% Z-Y-X 81 | \begin{tikzpicture}[scale=2,tdplot_main_coords,baseline=(current bounding box.north)] 82 | % \node[anchor=east](rectangle) at (0,0,0){\textcolor{blue}O}; 83 | % \node(rectangle) at (0,5.7,1.6){$\begin{aligned} 84 | % 1) &\text{ Vector: } \vr_{O\!P} \quad \text{vector from point $O$ to point $P$)}\\ 85 | % 2) &\text{ Position: } {}_A\vr_{O\!P} \quad \text{position vector from point $O$ to point $P$ expressed in frame $A$)}\\ 86 | % 3) &\text{ Active Rotation: } R^A(\theta): {}_A\color{blue}{\vr_{O\!P}} \color{black}\mapsto {}_A\color{green!50!black}{\vr_{O\!Q}} \quad \text{(rotates the vector $\vr_{O\!P}$)}\\ 87 | % 4) &\text{ Passive Rotation: } R^P(\theta): {}_A\color{blue}{\vr_{O\!P}} \color{black}\mapsto \color{red}{}_B\color{blue}{\vr_{O\!P}} \quad \text{(rotates the frame $(\ve_x^A,\ve_y^A,\ve_z^A)$)}\\ 88 | % 5) & R^{A^{-1}}(\vr) = R^P(\vr) \\ 89 | % \end{aligned}$}; 90 | % Set origin of main (body) coordinate system 91 | \coordinate (O) at (0,0,0); 92 | %\node[draw=none] at (0.5,-0.25,1) {3)}; 93 | \filldraw (0,0,0) circle (0.5pt); 94 | \node[draw=none,anchor=east] at (0.0,0,0) {$O$}; 95 | % Draw main coordinate system 96 | %\node[mark size=1pt,color=blue] at (1,1,0) {\pgfuseplotmark{*}}; 97 | \filldraw (1,1,0) circle (0.5pt); 98 | \draw[black, ,->] (0,0,0) -- (1,0,0) node[anchor=east]{$e_x^{I}$}; 99 | \draw[black, ,->] (0,0,0) -- (0,1,0) node[anchor= west]{$e_y^{I}$}; 100 | \draw[black, ,->] (0,0,0) -- (0,0,1) node[anchor=west]{$e_z^{I}$ \textcolor{red}{$e_{z}^B$}}; 101 | \draw[blue, very thick,->] (0,0,0) -- (1,1,0) node[anchor=south west]{\textcolor{black}{$P$}}; 102 | \node[anchor=north](rectangle) at (1,1,0) {$\color{black}{{}_I}\color{blue}{\vr_{O\!P}}, \color{red}{{}_B} \color{blue}{\vr_{O\!P}}$}; 103 | 104 | % Intermediate frame 1 105 | \tdplotsetrotatedcoords{\zRot}{0}{0} 106 | \draw[tdplot_rotated_coords,->, red] (0,0,0) -- (1,0,0) node[anchor=north]{$e_{x}^B$}; 107 | \draw[tdplot_rotated_coords,->, red] (0,0,0) -- (0,1,0) node[anchor=south]{$e_{y}^B$}; 108 | \draw[tdplot_rotated_coords,->, red] (0,0,0) -- (0,0,1) node[anchor=west]{}; 109 | \filldraw[tdplot_rotated_coords](1,1,0) circle (0.5pt); 110 | \draw[tdplot_rotated_coords,very thick,->, green!50!black] (0,0,0) -- (1,1,0) node[anchor=south west]{}; 111 | \node[tdplot_rotated_coords, draw=none, anchor=south west, yshift=-3pt] at (1,1,0) {$Q$}; 112 | \node[tdplot_rotated_coords, anchor=north west ](rectangle) at (1,1,0) {$\color{black}{{}_I}\color{green!50!black}{\vr_{O\!Q}}, {\color{red} {}_B} \color{green!50!black}{\vr_{O\!Q}}$}; 113 | %\tdplotsetrotatedthetaplanecoords{0} 114 | %\tdplotdrawarc[thick,tdplot_rotated_coords,->,color=gray]{(0,0,0)}{0.5}{0}{\yRot}{anchor=east,color=gray}{$\theta$} 115 | 116 | 117 | 118 | \tdplotsetrotatedthetaplanecoords{0} 119 | \tdplotdrawarc[thick,->,color=gray]{(0,0,0)}{.5}{0}{\zRot}{anchor=north east,color=gray}{$\theta$} 120 | \tdplotdrawarc[thick,->,color=gray]{(0,0,0)}{.5}{45}{45+\zRot}{anchor=north west,color=gray}{$\theta$} 121 | 122 | %\tdplotsetrotatedcoords{\zRot}{\yRot+90}{0} 123 | %\tdplotdrawarc[thick,tdplot_rotated_coords,->,color=gray]{(0,0,0)}{0.5}{90}{90+\xRot}{anchor=south,color=gray}{$\phi$} 124 | 125 | 126 | 127 | \end{tikzpicture} 128 | 129 | 130 | \end{document} 131 | -------------------------------------------------------------------------------- /doc/cheatsheet/pics/rpy.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/kindr/32800890d546e306f73c3ee3091fb6903452f925/doc/cheatsheet/pics/rpy.png -------------------------------------------------------------------------------- /doc/cheatsheet/references.bib: -------------------------------------------------------------------------------- 1 | 2 | @ARTICLE{Bloesch2016, 3 | author = {{Bloesch}, M. and {Sommer}, H. and {Laidlow}, T. and {Burri}, M. and 4 | {Nuetzi}, G. and {Fankhauser}, P. and {Bellicoso}, D. and {Gehring}, C. and 5 | {Leutenegger}, S. and {Hutter}, M. and {Siegwart}, R.}, 6 | title = "{A Primer on the Differential Calculus of 3D Orientations}", 7 | journal = {ArXiv e-prints}, 8 | archivePrefix = "arXiv", 9 | eprint = {1606.05285}, 10 | primaryClass = "cs.RO", 11 | keywords = {Computer Science - Robotics}, 12 | year = 2016, 13 | month = jun, 14 | adsurl = {http://adsabs.harvard.edu/abs/2016arXiv160605285B}, 15 | adsnote = {Provided by the SAO/NASA Astrophysics Data System} 16 | } 17 | 18 | @article{Iwamura2013, 19 | author = {Iwamura, Makoto and Nagao, Masafumi}, 20 | doi = {10.1007/s11044-012-9334-7}, 21 | file = {:home/gech/ETH/LeggedRobotics/Literature/mechanics/Hessian.pdf:pdf}, 22 | issn = {13845640}, 23 | journal = {Multibody System Dynamics}, 24 | keywords = {Closed loop systems,Hessian tensor,Jacobian matrix,Multibody dynamics,Relative coordinates}, 25 | number = {2}, 26 | pages = {173--184}, 27 | title = {{A method for computing the Hessian tensor of loop closing conditions in multibody systems}}, 28 | volume = {30}, 29 | year = {2013} 30 | } 31 | -------------------------------------------------------------------------------- /doc/doxygen/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # * Redistributions of source code must retain the above copyright 7 | # notice, this list of conditions and the following disclaimer. 8 | # * Redistributions in binary form must reproduce the above copyright 9 | # notice, this list of conditions and the following disclaimer in the 10 | # documentation and/or other materials provided with the distribution. 11 | # * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 12 | # names of its contributors may be used to endorse or promote products 13 | # derived from this software without specific prior written permission. 14 | # 15 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | # DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 19 | # Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 20 | # OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 21 | # GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 22 | # HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 23 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | cmake_minimum_required(VERSION 3.16.3) 27 | 28 | #--------------------------# 29 | # Documentation 30 | 31 | 32 | 33 | find_package(Doxygen) 34 | if(DOXYGEN) 35 | set(DOC_OUTPUT_DIR ${PROJECT_BINARY_DIR}/doc) 36 | set(DOXYGEN_IMAGE_DIRS "\"${CMAKE_CURRENT_SOURCE_DIR}/figures/\"" ) 37 | set(DOXYGEN_SOURCE_DIRS "${DOXYGEN_SOURCE_DIRS} \"${CMAKE_CURRENT_SOURCE_DIR}\" ") 38 | set(DOXYGEN_SOURCE_DIRS "${DOXYGEN_SOURCE_DIRS} \"${CMAKE_CURRENT_SOURCE_DIR}/../../include/\" ") 39 | 40 | 41 | set(HTML_EXTRA_FILES "${CMAKE_CURRENT_SOURCE_DIR}/../cheatsheet/cheatsheet_latest.pdf") 42 | set(HTML_DIR ${DOC_OUTPUT_DIR}/html) 43 | set(DOXYGEN_QUIET YES) 44 | make_directory(${HTML_DIR}) 45 | set( HAVE_DOT YES ) 46 | 47 | configure_file(${CMAKE_CURRENT_SOURCE_DIR}/doxygen.config.in ${DOC_OUTPUT_DIR}/doxygen.config IMMEDIATE) 48 | 49 | add_custom_target(kindr_doc ${DOXYGEN} ${DOC_OUTPUT_DIR}/doxygen.config) 50 | else(DOXYGEN) 51 | message(STATUS "Doxygen not found") 52 | endif(DOXYGEN) 53 | 54 | #-------------------------# 55 | -------------------------------------------------------------------------------- /doc/doxygen/figures/EulerAnglesXyz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/kindr/32800890d546e306f73c3ee3091fb6903452f925/doc/doxygen/figures/EulerAnglesXyz.png -------------------------------------------------------------------------------- /doc/doxygen/figures/EulerAnglesZyx.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/kindr/32800890d546e306f73c3ee3091fb6903452f925/doc/doxygen/figures/EulerAnglesZyx.png -------------------------------------------------------------------------------- /doc/doxygen/figures/rotation_active_passive.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/kindr/32800890d546e306f73c3ee3091fb6903452f925/doc/doxygen/figures/rotation_active_passive.png -------------------------------------------------------------------------------- /doc/doxygen/mainpage.dox: -------------------------------------------------------------------------------- 1 | /*! 2 | \mainpage Overview 3 | 4 | \section kindr Kindr 1.0.0 - Kinematics and Dynamics for Robotics 5 | This library provides interfaces and implementations of kinematic and dynamic quantities for robotics. 6 | 7 | \section source_code Source Code 8 | The source code is available at http://github.com/anybotics/kindr 9 | 10 | \section license License 11 | This library is Free Software and is licensed under BSD 3-Clause. 12 | 13 | \section acknowledgement Acknowledgement 14 | Involved people: 15 | - C. Dario Bellicoso 16 | - Michael Bloesch 17 | - Remo Diethelm 18 | - Peter Fankhauser 19 | - Paul Furgale 20 | - Christian Gehring 21 | - Michael Neunert 22 | - Hannes Sommer 23 | 24 | Contact: 25 | Remo Diethelm (rdiethelm ( at ) anybotics.com) 26 | */ 27 | 28 | 29 | 30 | //----------------------------------------------------------- 31 | 32 | /*! \page getting_started Getting started 33 | 34 | This library comes a long with a Cheatsheet, which shows the mathematical notation and should help you to understand what is implemented. 35 | 36 | This is a very short guide on how to get started with this library. 37 | 38 | \section requirements Requirements 39 | This library is written in C++11. 40 | 41 | - [Eigen 3.2.0](http://eigen.tuxfamily.org) (Older versions might also work) 42 | - GCC 4.7 is required at the minimum. 43 | - CMake 2.8.3 is required at the minimum. 44 | 45 | 46 | \section install Installation 47 | \subsection build_lib_cmake Build the Library with CMake 48 | 49 | \code{.sh} 50 | mkdir build 51 | cd build 52 | cmake .. 53 | make 54 | \endcode 55 | 56 | The library can additionally be installed by 57 | \code{.sh} 58 | sudo make install 59 | \endcode 60 | This will copy the file FindKindr.cmake to the module path of CMake. 61 | 62 | \subsection build_lib_ros Install the Library from Debian Package for ROS 63 | 64 | The maintainers of this project provide binary packages for ROS and Ubuntu 65 | LTS releases. To install these packages, you may follow these instructions: 66 | 67 | - Add the project PPA to your APT sources by issuing 68 | 69 | \code{.sh} 70 | sudo add-apt-repository ppa:ethz-asl/common 71 | \endcode 72 | 73 | on the command line 74 | 75 | - To re-synchronize your package index files, run 76 | 77 | \code{.sh} 78 | sudo apt-get update 79 | \endcode 80 | 81 | - Install all project packages and their dependencies through 82 | 83 | \code{.sh} 84 | sudo apt-get install ros-indigo-kindr-* 85 | \endcode 86 | 87 | or selected packages using your favorite package management tool. The package will be installed to the default ROS directory. 88 | 89 | 90 | \subsection build_lib_catkin Build the Library with Catkin 91 | 92 | Build kindr with [catkin](wiki.ros.org/catkin): 93 | 94 | \code{.sh} 95 | cd ~/catkin_ws/src 96 | git clone git@github.com:anybotics/kindr.git 97 | catkin_make_isolated -C ~/catkin_ws 98 | \endcode 99 | 100 | or with [catkin command line tools](http://catkin-tools.readthedocs.org): 101 | 102 | \code{.sh} 103 | cd ~/catkin_ws/src 104 | git clone git@github.com:anybotics/kindr.git 105 | catkin build -w ~/catkin_ws kindr 106 | \endcode 107 | 108 | Kindr can be included in your catkin project with: 109 | Add the following to your *CMakeLists.txt*: 110 | 111 | \code{.sh} 112 | find_package(catkin COMPONENTS kindr) 113 | include_directories(${catkin_INCLUDE_DIRS}) 114 | \endcode 115 | 116 | And to your *package.xml*: 117 | 118 | \code{.xml} 119 | 120 | kindr 121 | 122 | \endcode 123 | 124 | \subsection build_doc Build this Documentation 125 | Doxygen needs to be installed to create this documentation. 126 | 127 | \code{.sh} 128 | mkdir build 129 | cd build 130 | cmake .. 131 | make doc 132 | \endcode 133 | The doxygen documentation can be found here: 134 | doc/doxygen/doc/html/index.html 135 | 136 | 137 | \subsection build_tests Build Unit Tests 138 | The unit tests depend on google tests. 139 | The GTests are built as soon as the folder gtest exists in the root folder. 140 | 141 | Download and use GTest: 142 | \code{.sh} 143 | wget http://googletest.googlecode.com/files/gtest-1.7.0.zip 144 | unzip gtest-1.7.0.zip 145 | ln -s gtest-1.7.0 gtest 146 | mkdir build 147 | cd build 148 | cmake .. -DBUILD_TEST=True 149 | make 150 | \endcode 151 | 152 | 153 | \section sec_continue How to Continue 154 | See \ref page_users "Users" to learn how to use this library. 155 | 156 | See \ref page_developers "Developers" to learn how to develop this library. 157 | */ 158 | 159 | //----------------------------------------------------------- 160 | 161 | /*! 162 | \page page_users Users 163 | 164 | Check the cheatsheet to understand the math behind. 165 | 166 | \section orientation_pose Rotations and Poses 167 | - \subpage page_rotations 168 | - \subpage page_poses 169 | 170 | \section velocities Time Derivatives of Rotations and Poses 171 | - \subpage page_rdiff 172 | - \subpage page_posesdiff 173 | 174 | \section misc Mathematical Tools 175 | - \subpage page_quaternion 176 | 177 | \section vectors Vectors 178 | - \subpage page_vector 179 | 180 | */ 181 | 182 | //----------------------------------------------------------- 183 | 184 | /*! 185 | \page page_developers Developers 186 | 187 | 188 | \section coding_style Coding Style 189 | The coding style of this library must be in compliance with the coding style guidelines of ANYbotics. 190 | 191 | Developers are encouraged to make use of the new features of C++11. 192 | 193 | */ 194 | 195 | 196 | //----------------------------------------------------------- 197 | 198 | -------------------------------------------------------------------------------- /doc/doxygen/page_pose.dox: -------------------------------------------------------------------------------- 1 | /** 2 | * \defgroup poses Poses 3 | * \see \ref page_poses "Chapter Poses" 4 | */ 5 | 6 | /*! \page page_poses Poses 7 | 8 | This library defines an \ref poses_interface "interface" for a pose (position and orientation) of a rigid body or a (displacement) frame to enable different representations of a pose (homogeneous transformation, screw motion, etc.). 9 | 10 | \tableofcontents 11 | 12 | \section poses_interface Generic Interface 13 | The class kindr::PoseBase serves as an interface for a pose of a rigid body, i.e. the position and orientation of a rigid body. All types of representations of a pose, such as homogeneous transformations and screw motions, are derived from this base class. 14 | 15 | 16 | \section poses_implementations Parameterizations 17 | 18 | \subsection poses_implementations_homtransform Homogeneous Transformation 19 | \f$\boxed{\begin{bmatrix}{}_I\mathbf{r}_{I\!P} \\ 1 \end{bmatrix} = \mathbf{T}_{I\!B} \begin{bmatrix}{}_B\mathbf{r}_{B\!P}\\ 1 \end{bmatrix}, \quad \mathbf{T}_{I\!B} = \begin{bmatrix} 20 | \mathbf{C}_{I\!B} & {}_I\mathbf{r}_{I\!B} \\ 21 | 0^T & 1 \\ 22 | \end{bmatrix}, 23 | \quad 24 | \mathbf{T}_{I\!B}^{-1} =\mathbf{T}_{B\!I} = \begin{bmatrix} 25 | \mathbf{C}_{I\!B}^T & -\mathbf{C}_{I\!B}^T {}_I\mathbf{r}_{I\!B} \\ 26 | 0^T & 1 \\ 27 | \end{bmatrix}}\f$ 28 | 29 | 30 | The following typedefs are provided for convenience: 31 | - \ref kindr::HomTransformQuatD "HomTransformQuatD" for primitive type double (stores internally a rotation quaternion) 32 | - \ref kindr::HomTransformQuatF "HomTransformQuatF" for primitive type float (stores internally a rotation quaternion) 33 | - \ref kindr::HomTransformMatrixD "HomTransformMatrixD" for primitive type double (stores internally a rotation matrix) 34 | - \ref kindr::HomTransformMatrixF "HomTransformMatrixF" for primitive type float (stores internally a rotation matrix) 35 | 36 | \subsubsection poses_implementation_homtransform Create a Homogeneous Transformation 37 | \code{.cpp} 38 | Position3D position; 39 | RotationQuaternionD rotation; 40 | HomTransformQuatD transform1; // identity tranformation 41 | HomTransformQuatD transform2(position, rotation); // create with position and rotation 42 | \endcode 43 | 44 | \subsubsection poses_implementation_homtransform Use a Homogeneous Transformation 45 | \code{.cpp} 46 | 47 | Eigen::Matrix matrix = transform1.getTransformationMatrix(); // gets the 4x4 transformation matrix 48 | transform1.setIdentity(); // sets it to identity 49 | transform2.invert(); // invert transformation 50 | Eigen::Vector3d vector_transform = transform2.rotate(Eigen::Vector3d(1.0,2.0,3.0)); // tranform a vector 51 | Eigen::Vector3d vector_invtransform = transform2.inverseTransform(Eigen::Vector3d(1.0,2.0,3.0)); // reverse tranform a vector 52 | HomTransformQuatD transform3 = transform2*transform1; // composition of transformations 53 | 54 | \endcode 55 | 56 | 57 | 58 |
59 |
60 |
61 |
62 |
63 |
64 |
65 |
66 |
67 |
68 |
69 |
70 |
71 |
72 |
73 | 74 | 75 | 76 | */ -------------------------------------------------------------------------------- /doc/doxygen/page_posediff.dox: -------------------------------------------------------------------------------- 1 | /** 2 | * \defgroup posesdiff Time Derivatives of Poses 3 | * \see \ref page_posesdiff "Chapter Time Derivatives of Poses" 4 | */ 5 | 6 | /*! \page page_posesdiff Time Derivatives of Poses 7 | 8 | This library defines an \ref posesdiff_interface "interface" for the time derivative of a pose (position and orientation) of a rigid body or a (displacement) frame to enable different representations of a time derivative of a pose (twist, etc.). 9 | 10 | \tableofcontents 11 | 12 | \section posesdiff_interface Generic Interface 13 | The class kindr::PoseDiffBase serves as an interface for the time derivative of a pose of a rigid body, i.e. the position and orientation of a rigid body. All types of representations of time derivatives of a pose, such as twist motions, are derived from this base class. 14 | 15 | 16 | \section posesdiff_implementations Parameterizations 17 | 18 | \subsection posesdiff_implementations_twist Twist 19 | 20 | The class kindr::Twist implements a 6d twist: 21 | 22 | \f$\boxed{\mathbf{\phi} = \begin{pmatrix} {}_B\mathbf{v}_B \\ {}_B\mathbf{\omega}_{I\!B}\end{pmatrix}}\f$ 23 | 24 | where \f${}_B\mathbf{v}_B\f$ is the linear velocity in body-fixed frame and \f${}_B\mathbf{\omega}_{I\!B}\f$ is the local angular velocity. 25 | 26 | 27 |
28 |
29 |
30 |
31 |
32 |
33 |
34 |
35 |
36 |
37 |
38 |
39 |
40 |
41 |
42 | 43 | */ -------------------------------------------------------------------------------- /doc/doxygen/page_quaternion.dox: -------------------------------------------------------------------------------- 1 | /** 2 | * \defgroup quaternions Quaternions 3 | * \see \ref page_quaternion "Chapter Quaternions" 4 | */ 5 | 6 | /*! \page page_quaternion Quaternions 7 | This library defines an \ref quaternion_interface "interface" for quaternions to enable different \ref quaternion_implementations "implementations". 8 | 9 | Note that these quaternions should not be used to represent rotations! Use a \ref kindr::RotationQuaternion "RotationQuaternion" instead as explained on page \ref page_rotations "Rotations". 10 | 11 | \section quaternion_interface Interfaces 12 | The interfaces are declared in \ref QuaternionBase.hpp. 13 | \subsection generic_quaternion Generic Quaternion 14 | The class kindr::QuaternionBase defines the \em interface for a Hamiltonian generic quaternion defined as: 15 | 16 | \f$\boxed{\begin{aligned}Q &= q_0 + q_1 i + q_2 j + q_3 k \in \mathbb{H}, \quad q_i \in \mathbb{R} \\ 17 | i^2 &= j^2=k^2 = ijk = -1 \\ 18 | \end{aligned}}\f$ 19 | 20 | The interface defines generic functions, such as: 21 | - inversion (kindr::QuaternionBase::invert()) 22 | - conjugation (kindr::QuaternionBase::conjugate()) 23 | - multiplication (kindr::QuaternionBase::operator*()) 24 | - comparison (kindr::QuaternionBase::operator==()) 25 | 26 | \subsection unit_quternion Unit Quaternion 27 | The class kindr::UnitQuaternionBase defines the \em interface for a Hamiltonian unit quaternion defined as: 28 | 29 | \f$\boxed{\begin{aligned}P &= p_0 + p_1 i + p_2 j + p_3 k \in \mathbb{H}, \quad p_i \in \mathbb{R} \\ 30 | i^2 &= j^2=k^2 = ijk = -1, \quad \lVert P \rVert= \sqrt{p_0^2 + p_1^2 + p_2^2 + p_3^2} = 1 \\ 31 | \end{aligned}}\f$ 32 | 33 | There are two reasons why there is an extra class for unit quaternions: 34 | - Mathematical properties of a unit quaternion, such as the inverse is equal to the conjugate, can be exploited to increase computational speed. 35 | - The unit length can be guaranteed. 36 | 37 | \section quaternion_implementations Implementation 38 | 39 | Eigen provides a Eigen::Quaternion, which is used as a primitive for a quaternion. 40 | 41 | To use the quaternions based on Eigen, include the following header: 42 | \code{.cpp} 43 | #include 44 | \endcode 45 | 46 | \subsection generic_quaternion_eigen Generic Quaternion 47 | 48 | The class kindr::Quaternion implements a Hamiltonian generic quaternion. 49 | 50 | The coefficients are defined as 51 | 52 | \f$\boxed{Q = w + x i + yj + z k}\f$ 53 | 54 | The following two typedefs are provided for convenience: 55 | - \ref kindr::QuaternionF "QuaternionF" for float 56 | - \ref kindr::QuaternionD "QuaternionD" for double 57 | 58 | Example code to create a quaternion: 59 | \code{.cpp} 60 | namespace quat = kindr::eigen_impl; // select implementation based on Eigen 61 | quat::QuaternionD quat1; // creates a quaternion with all coefficients equal to zero 62 | quat::QuaternionD quat2(1.0, 2.0, 3.0, 4.0); //creates a quaternion with w=1, x=2, y=3, z=4 63 | \endcode 64 | 65 | \subsection unit_quaternion_eigen Unit Quaternion 66 | 67 | The class kindr::UnitQuaternion implements a Hamiltonian unit quaternion. 68 | 69 | The following two typedefs are provided for convenience: 70 | - \ref kindr::UnitQuaternionF "UnitQuaternionF" for float 71 | - \ref kindr::UnitQuaternionD "UnitQuaternionD" for double 72 | 73 | 74 | The class only checks if the quaternion has unit length if the code is built with debugging symbols, i.e. #define NDEBUG exists. If the unit quaternion has not unit length, an exception will be thrown. 75 | 76 | \subsection generic_unit_quaternion_eigen Conversion between Generic Quaternion and Unit Quaternion 77 | 78 | Conversion by constructor: 79 | \code{.cpp} 80 | UnitQuaternionD uquat; 81 | QuaternionD quat(uquat); 82 | UnitQuaternionD uquat2(quat); // error (not allowed) 83 | UnitQuaternionD uquat3(quat.toUnitQuaternion()); // normalizes the quaternion 84 | \endcode 85 | 86 | Conversion by assignment: 87 | \code{.cpp} 88 | UnitQuaternionD uquat; 89 | QuaternionD quat; 90 | quat = uquat; 91 | uquat = quat; // error (not allowed) 92 | uquat = quat.toUnitQuaternion(); // normalizes the quaternion 93 | \endcode 94 | 95 | 96 | 97 |
98 |
99 |
100 |
101 |
102 |
103 |
104 |
105 |
106 |
107 |
108 |
109 |
110 |
111 |
112 | 113 | 114 | 115 | */ -------------------------------------------------------------------------------- /doc/doxygen/page_rotationdiff.dox: -------------------------------------------------------------------------------- 1 | /*! \page page_rdiff Time Derivatives of Rotations 2 | 3 | This library defines an \ref rdiff_interface "interface" for time derivatives of rotations to enable different representations of the time derivative (angular velocity, time derivatives of Euler angles, time derivative of quaternion, etc.). 4 | The section \ref rdiff_interface "Interface" describes the functionalities, whereas the section \ref rdiff_implementations "Parameterization" shows some examples. 5 | 6 | \tableofcontents 7 | 8 | \section rdiff_interface Generic Interface 9 | The class kindr::RotationDiffBase serves as an interface for the time derivative of a rotation. All types of representations of a time derivative of a rotation, such as angular velocity and time derivatives of Euler angles, are derived from this base class. 10 | 11 | \subsection rdiff_interfaces_otationdiff_type Type of a Time Derivative of a Rotation 12 | The type of a rotation is defined by 13 | - the \ref rdiff_interfaces_representations "kind of parameterization" (derivative of quaternion, rotation matrix, etc.), 14 | - the primitive data type of the parameters (float/double, etc.) and 15 | 16 | \subsection rdiff_interfaces_rotationdiff_constructor Construction of a Time Derivative of a Rotation 17 | The default constructor always initializes all derivatives with zero. 18 | 19 | \subsection rdiff_zero Zero Time Derivatives 20 | All derivatives can be set to zero by the function \ref kindr::RotationDiffBase::setZero() "setZero()". 21 | 22 | \subsection rdiff_addition_and_subtraction Addition and Subtraction 23 | Addition and subtraction of the time derivatives are provided by the kindr::RotationDiffBase::operator+ and kindr::RotationDiffBase::operator-, respectively. 24 | 25 | \subsection rdiff_conversion Conversion between Time Derivatives of Rotations 26 | There are three kinds of conversions: 27 | - Conversion between different representations, i.e. between different parameterizations (e.g. rotation quaternion - angle-axis) 28 | - Conversion between different primitive data types (e.g. float - double) 29 | 30 | \subsubsection rdiff_interfaces_conversion_representations Conversion between Representations 31 | The following methods allow to convert between different representations of a rotation: 32 | \code{.cpp} 33 | LocalAngularVelocityD angularVelocity; // representation of the time derivative 34 | RotationQuaternionD rotationQuaternion; // the rotation the derivative is taken 35 | RotationQuaternionDiffD rotationQuaternionDiff(rotationQuaternion, angularVelocity); // convert using the constructor 36 | rotationQuaternionDiff = angularVelocity.cast(rotationQuaternion); // convert using the cast method 37 | 38 | \endcode 39 | 40 | \subsubsection rdiff_interfaces_conversion_primtype Conversion between Primitive Data Types 41 | Not yet supported. 42 | 43 | \subsubsection rdiff_interfaces_representations Representations of Time Derivatives of Rotations 44 | The following representations are currently provided by the library: 45 | 46 | \subsection rdiff_interfaces_angularvelocity Local Angular Velocity 47 | The class kindr::LocalAngularVelocity implements the local angular velocity in 3D-Space. 48 | The angular velocity expressed in body-fixed frame can be stated as: 49 | 50 | \f$\boxed{{}_B\mathbf{\omega}_{I\,B} = \begin{bmatrix} \omega_x \\ \omega_y \\ \omega_z \end{bmatrix}}\f$ 51 | 52 | It stores the three coordinates of the angular velocity in Eigen::Matrix. 53 | 54 | The following two typedefs are provided for convenience: 55 | - \ref kindr::AngularVelocityD "AngularVelocityD" for primitive type double 56 | - \ref kindr::AngularVelocityF "AngularVelocityF" for primitive type float 57 | 58 | An angular velocity can be created as follows: 59 | \code{.cpp} 60 | double w_x = 1.0; 61 | double w_y = 2.0; 62 | double w_z = 3.0; 63 | LocalAngularVelocityD velocity1(); // creates a 3D-Velocity with all coordinates equal to zero 64 | LocalAngularVelocityD velocity2(w_x, w_y, w_z); // initializes the three coordinates 65 | LocalAngularVelocityD velocity3(Eigen::Vector3d(w_x, w_y, w_z)); // creates a velocity from an Eigen vector 66 | \endcode 67 | 68 | The coordinates can be obtained as follows: 69 | \code{.cpp} 70 | w_x = velocity1.x(); 71 | w_y = velocity1.y(); 72 | w_z = velocity1.z(); 73 | Eigen::Vector3d vector = velocity1.toImplementation(); // [w_x;w_y;w_z] 74 | \endcode 75 | 76 | Operations with angular velocities: 77 | \code{.cpp} 78 | LocalAngularVelocityD velocity4 = velocity1 + velocity2; // addition 79 | LocalAngularVelocityD velocity3 -= velocity1; // subtraction 80 | std::cout << "Velocity 3 is: " << velocity3; // print velocity 81 | \endcode 82 | 83 | \subsection rdiff_interfaces_rotationquaterniondiff Time Derivative of Rotation Quaternion 84 | The class kindr::RotationQuaternionDiff implements the time derivative of a Hamiltonian unit quaternion representation of a rotation: 85 | 86 | \f$\boxed{\begin{aligned}\dot{P} &= p_0 + p_1 i + p_2 j + p_3 k \in \mathbb{H}, \quad p_i \in \mathbb{R} \\ 87 | i^2 &= j^2=k^2 = ijk = -1 \\ 88 | \end{aligned}}\f$ 89 | 90 | \subsection rdiff_interfaces_rotationmatrixdiff Time Derivative of Rotation Matrix 91 | The class kindr::RotationMatrixDiff implements the time derivative of a 3x3 rotation matrix representation of a rotation: 92 | 93 | \f$\boxed{\dot{C} = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix}}\f$ 94 | 95 | \subsection rdiff_interfaces_eulerangles_zyx_diff Time derivative of Euler Angles ZYX 96 | The class kindr::EulerAnglesZyxDiff implements the time derivatives of the Euler angles with the Z-Y-X convention: 97 | 98 | \f$\boxed{(\dot{z}, \dot{y}, \dot{x} ),}\f$ 99 | 100 | where \f$\dot{z}\f$ is the time derivative of the yaw angle, \f$\dot{y}\f$ is the time derivative of the pitch angle, and \f$\dot{x}\f$ is the time derivative of the roll angle. 101 | 102 | \subsection rdiff_interfaces_eulerangles_xyz_diff Time Derivative of Euler Angles XYZ 103 | The class kindr::EulerAnglesXyzDiff implements the time derivatives of the Euler angles with the X-Y-Z convention: 104 | 105 | \f$\boxed{(\dot{x}, \dot{y}, \dot{z},}\f$ 106 | 107 | where \f$\dot{z}\f$ is the time derivative of the yaw angle, \f$\dot{y}\f$ is the time derivative of the pitch angle, and \f$\dot{x}\f$ is the time derivative of the roll angle. 108 | 109 | 110 | 111 | 112 |
113 |
114 |
115 |
116 |
117 |
118 |
119 |
120 |
121 |
122 |
123 |
124 |
125 |
126 |
127 | 128 | 129 | 130 | */ -------------------------------------------------------------------------------- /doc/doxygen/page_vector.dox: -------------------------------------------------------------------------------- 1 | /** 2 | * \defgroup vectors Vectors 3 | */ 4 | 5 | /*! \page page_vector Vectors 6 | 7 | This library defines a \ref vectors_interface "interface" for a vector. 8 | It is possible to assign a certain physical type to the vector, such as position or velocity. 9 | The section \ref vectors_interface "Interface" describes the functionalities, whereas the section \ref vectors_implementations "Implementation" shows some examples. 10 | 11 | \tableofcontents 12 | 13 | \section vectors_interface Interface 14 | The class kindr::VectorBase serves as an interface for a vector. 15 | Vectors of all physical types are derived from this base class. 16 | 17 | \subsection physical_quantities Physical quantities: 18 | The following representations are currently provided by the library: 19 | 20 | - Typeless 21 | 22 | - Time 23 | - Mass 24 | - Inertia 25 | - Power 26 | - Energy 27 | 28 | - Jerk 29 | - Acceleration 30 | - Velocity 31 | - Position 32 | - Force 33 | - Momentum 34 | 35 | - AngularJerk 36 | - AngularAcceleration 37 | - AngularVelocity 38 | - Angle 39 | - Torque 40 | - AngularMomentum 41 | 42 | \subsection vectors_constructor Constructor 43 | The default constructor always initializes all coordinates equal to zero. 44 | Other (explicit) constructors use another vector or the implementation type. 45 | If the dimension of the vector is three, there is a constructor which accepts x, y and z as arguments. 46 | 47 | \subsection vectors_operators Operators 48 | All possible operators (+, -, *, /, +=, -=, *=, /=, (), <<) can be used for a vector object. 49 | However it is not possible to add or subtract vectors of different physical types. 50 | Elementwise multiplication and division is only possible for predefined triples of physical types. 51 | The \ref kindr::VectorBase::dot() "dot" product can be used, as well as the \ref kindr::VectorBase::cross() "cross" product (for vectors of dimension three). 52 | 53 | \subsection vectors_other_methods Other methods 54 | All coordinates can be set to zero by the function \ref kindr::VectorBase::setZero() "setZero()". 55 | The vector can be normalized with \ref kindr::VectorBase::normalize() "normalize()". 56 | There are methods which return the \ref kindr::VectorBase::max() "biggest" or the \ref kindr::VectorBase::min() "smallest" element, the \ref kindr::VectorBase::sum() "sum" or the \ref kindr::VectorBase::mean() "mean" of the entries. 57 | If the dimension of the vector is three, the coordinates can be accessed by x(), y() and z(). 58 | 59 | \section vectors_implementations Implementations 60 | All types can be included by 61 | \code{.cpp} 62 | #include 63 | \endcode 64 | and lie within the namespace kindr. 65 | 66 | 67 | 68 | 69 | 70 |
71 |
72 |
73 |
74 |
75 |
76 |
77 |
78 |
79 |
80 |
81 |
82 |
83 |
84 |
85 | 86 | 87 | 88 | */ -------------------------------------------------------------------------------- /doc/notes_kindr_1.0.0.md: -------------------------------------------------------------------------------- 1 | # Kindr 1.0.0 - Notes 2 | 3 | # Changes from Kindr 0.0.1 4 | 5 | * C_BI.boxPlus(dt * B_w_IB) ==> C_BI.boxPlus(dt * C_IB * B_w_IB) 6 | * C_BI.boxMinus(dt * B_w_IB) ==> -C_BI.boxMinus(dt * B_w_IB) 7 | * setFromVectors(v1, v2) ==> setFromVectors(v2, v1) = setFromVectors(v1, v2).invert() 8 | * EulerAnglesZyx(z, y, x) ==> EulerAnglesZyx(-z, -y,-x) 9 | * EulerAnglesXyz(x, y, z) ==> EulerAnglesXyz(-x, -y,-z) 10 | 11 | # Kindr 1.0.0 12 | * Convention: Hamilton, passive 13 | 14 | # Gazebo 15 | 16 | * Convention: Hamilton, passive 17 | * Convention tests can be found in the unit tests of package kindr_gazebo in repo any_gazebo. 18 | * Gazebos's gazebo::math::Quaternion has the same convention as kindr. 19 | * Gazebo's output: 20 | - (I_r, C_IB) = link->GetWorldPose(); (position expressed in inertial frame, rotation from base to interial frame) 21 | - B_v = link->GetRelativeLinearVel(); (linear velocity in base frame 22 | - B_w_IB = link->GetRelativeAngularVel(); (local angular velocity) 23 | 24 | # ROS TF, Rviz 25 | 26 | * Convention: Hamilton, passive, RViz wants TF message with rotation C_IB 27 | * Convention tests can be found in the unit tests of package kindr_ros in repo kindr_ros. 28 | * tf::Quaternion has the same convention as kindr. 29 | * tf::Matrix3x3 (rotation matrix) has the same convention as kindr 30 | * tf::Transform behaves the same as kindr::HomogeneousTransformationPosition3RotationQuaternionD 31 | 32 | 33 | # RBDL 34 | 35 | * Convention: not consistent (mostly Hamiltonian) 36 | * Conversion tests can be found in the unit tests of package rbdl in the monorepo. 37 | * RBDL's RigidBodyDynamics::Math::Quaternion has the same convention as kindr, but the resulting rotation matrix from toMatrix() is inverted! 38 | * RBDL's rotation of RigidBodyDynamics::Math::SpatialTransform has the same convention as kindr 39 | * Attention: RBDL inverts the quaternion in the generalized coordinates! 40 | 41 | ## Calculations in RBDL 42 | 43 | ### Transformation X_base 44 | std::vector X_base 45 | Comments in code: "Transformation from the base to bodies reference frame." 46 | 47 | According to *CalcBodyToBaseCoordinates* (see below), the following position and orientation is stored: 48 | 49 | X_base.r = W_r_WB 50 | X_base.E = C_BW 51 | 52 | 53 | ### CalcBodyToBaseCoordinates 54 | Base Coordinates = World Coordinates 55 | Comments in code: 56 | * "Returns the base coordinates of a point given in body coordinates. 57 | * "3-D vector with coordinates of the point in base coordinates" 58 | 59 | The calc should be: W_r_WP = W_r_WB + C_WB*B_r_BP 60 | 61 | The code says: 62 | 63 | ``` 64 | Matrix3d body_rotation = model.X_base[body_id].E.transpose(); 65 | Vector3d body_position = model.X_base[body_id].r; 66 | return body_position + body_rotation * point_body_coordinates; 67 | ``` 68 | 69 | X_base.E should correspond to C_BW 70 | 71 | 72 | ### CalcBaseToBodyCoordinates 73 | 74 | * "Returns the body coordinates of a point given in base coordinates." 75 | * "Returns a 3-D vector with coordinates of the point in body coordinates" 76 | 77 | B_r_BP = C_BW * (W_r_WP - W_r_WB) 78 | 79 | ``` 80 | Matrix3d body_rotation = model.X_base[body_id].E; 81 | Vector3d body_position = model.X_base[body_id].r; 82 | return body_rotation * (point_base_coordinates - body_position); 83 | ``` 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | -------------------------------------------------------------------------------- /gtesting.txt: -------------------------------------------------------------------------------- 1 | ----------------- RotationBase ----------------- 2 | 3 | Main functionality: 4 | - Constructor 5 | - Access to derived 6 | - Inversion 7 | - Setters (identity) 8 | - Unique (getter and setter) 9 | - Passive/Active forms 10 | - Concatenation 11 | - Comparison (equality) 12 | - Rotation of a vector (normal and inverse) 13 | 14 | Derived classes: 15 | - AngleAxisBase (assignment || toImplementation, angle, axis, <<) 16 | - RotationQuaternionBase (assignment || toUnitQuaternion, bracket operator, conjugation, norm) 17 | - RotationMatrixBase (assignment || transposition, determinant, toImplementation, matrixAccess, <<) 18 | - EulerAnglesBase -> Xyz,Zyx (assignment || toImplementation, angles, <<) 19 | 20 | Internal Stuff: 21 | - UsageConvertionTraits (getActive, getPassive) 22 | - get_other_usage (OtherUsage) 23 | - ConversionTraits (convert) 24 | - ComparisonTraits (isEqual,isNear) 25 | - MultiplicationTraits (mult) 26 | - RotationTraits (rotate) 27 | - get_matrix3X (IndexType, Matrix3X) 28 | 29 | ----------------- QuaternionBase ----------------- 30 | 31 | Main functionality: 32 | - Inversion 33 | - Conjugation 34 | - Access to derived 35 | - Multiplication 36 | - Comparison (equality) 37 | - Printing 38 | 39 | Derived classes: 40 | - Quaternion (Constructor, bracket operator, toImplementation, component access, normalization, toUnitQuaternion) 41 | - Unit Quaternion (Constructor, assignment, bracket operator, component access, norm, toImplementation) 42 | 43 | Internal Stuff: 44 | - ConversionTraits (convert) 45 | - MultiplicationTraits (mult) 46 | - ComparisonTraits (isEqual) 47 | -------------------------------------------------------------------------------- /include/kindr/Core: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 20 | * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 21 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 22 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | -------------------------------------------------------------------------------- /include/kindr/common/common.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 20 | * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 21 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 22 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | namespace kindr { 39 | 40 | /*! \brief Floating-point modulo 41 | * 42 | * The result (the remainder) has same sign as the divisor. 43 | * Similar to matlab's mod(); Not similar to fmod(): floatingPointModulo(-3,4)= 1 fmod(-3,4)= -3 44 | */ 45 | template 46 | T floatingPointModulo(T x, T y) 47 | { 48 | static_assert(!std::numeric_limits::is_exact , "floatingPointModulo: floating-point type expected"); 49 | 50 | if (y == 0.0) 51 | return x; 52 | 53 | double m= x - y * floor(x/y); 54 | 55 | // handle boundary cases resulted from floating-point cut off: 56 | 57 | if (y > 0) // modulo range: [0..y) 58 | { 59 | if (m>=y) // mod(-1e-16 , 360. ): m= 360. 60 | return 0; 61 | 62 | if (m<0 ) 63 | { 64 | if (y+m == y) 65 | return 0 ; // just in case... 66 | else 67 | return y+m; // Mod(106.81415022205296 , 2*M_PI ): m= -1.421e-14 68 | } 69 | } 70 | else // modulo range: (y..0] 71 | { 72 | if (m<=y) // mod(1e-16 , -360. ): m= -360. 73 | return 0; 74 | 75 | if (m>0 ) 76 | { 77 | if (y+m == y) 78 | return 0 ; // just in case... 79 | else 80 | return y+m; // mod(-106.81415022205296, -2*M_PI): m= 1.421e-14 81 | } 82 | } 83 | 84 | return m; 85 | } 86 | 87 | //! wrap angle to [x1..x2) 88 | template 89 | inline T wrapAngle(T angle, T x1, T x2) 90 | { 91 | return floatingPointModulo(angle-x1, x2-x1) + x1; 92 | } 93 | 94 | //! wrap angle to [-PI..PI) 95 | template 96 | inline T wrapPosNegPI(T angle) 97 | { 98 | return floatingPointModulo(angle + T(M_PI), T(2.0*M_PI) ) - M_PI; 99 | } 100 | 101 | //! wrap angle to [0..2*PI) 102 | template 103 | inline T wrapTwoPI(T angle) 104 | { 105 | return floatingPointModulo(angle, T(2.0*M_PI)); 106 | } 107 | 108 | 109 | template 110 | static inline void setUniformRandom(Eigen::Matrix& vector, PrimType_ min, PrimType_ max) { 111 | std::random_device rand_dev; 112 | std::mt19937 generator(rand_dev()); 113 | std::uniform_real_distribution distr(min, max); 114 | for (int i=0; i 127 | class MultiplicationTraits { 128 | public: 129 | // inline static Left_ mult(const Left_& lhs, const Right_& rhs); 130 | }; 131 | 132 | /*! \brief Comparison traits 133 | * \class ComparisonTraits 134 | * (only for advanced users) 135 | */ 136 | template 137 | class ComparisonTraits { 138 | public: 139 | // inline static bool isEqual(const Left_& left, const Right_& right); 140 | }; 141 | 142 | 143 | /*! \class get_scalar 144 | * \brief Gets the primitive of the vector. 145 | */ 146 | template 147 | class get_scalar { 148 | public: 149 | // typedef PrimType Scalar; 150 | }; 151 | 152 | 153 | template 154 | class GenericNumTraits { 155 | public: 156 | enum { 157 | IsInteger = std::numeric_limits::is_integer, 158 | IsSigned = std::numeric_limits::is_signed, 159 | }; 160 | 161 | typedef T Scalar; 162 | 163 | 164 | static inline Scalar epsilon() { return std::numeric_limits::epsilon(); } 165 | static inline Scalar dummy_precision() 166 | { 167 | // make sure to override this for floating-point types 168 | return Scalar(0); 169 | } 170 | static inline T highest() { return (std::numeric_limits::max)(); } 171 | static inline T lowest() { return IsInteger ? (std::numeric_limits::min)() : (-(std::numeric_limits::max)()); } 172 | 173 | }; 174 | 175 | template 176 | class NumTraits : GenericNumTraits { 177 | public: 178 | }; 179 | 180 | template<> 181 | class NumTraits : GenericNumTraits 182 | { 183 | public: 184 | static inline float dummy_precision() { return 1e-5f; } 185 | }; 186 | 187 | template<> 188 | class NumTraits : GenericNumTraits 189 | { 190 | public: 191 | static inline double dummy_precision() { return 1e-12; } 192 | }; 193 | 194 | template<> 195 | class NumTraits : GenericNumTraits 196 | { 197 | public: 198 | static inline long double dummy_precision() { return 1e-15l; } 199 | }; 200 | 201 | 202 | } // namespace internal 203 | } // namespace kindr 204 | 205 | -------------------------------------------------------------------------------- /include/kindr/common/source_file_pos.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary fokindrs, with or without 6 | * modification, are pekindritted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary fokindr must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written pekindrission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 20 | * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 21 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 22 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | #ifndef KINDR_COMMON_SOURCE_FILE_POS_HPP 29 | #define KINDR_COMMON_SOURCE_FILE_POS_HPP 30 | 31 | #include 32 | #include 33 | #include 34 | // A class and macro that gives you the current file position. 35 | 36 | namespace kindr { 37 | namespace internal { 38 | 39 | class source_file_pos 40 | { 41 | public: 42 | std::string function; 43 | std::string file; 44 | int line; 45 | 46 | source_file_pos(std::string function, std::string file, int line) : 47 | function(function), file(file), line(line) {} 48 | 49 | operator std::string() 50 | { 51 | return toString(); 52 | } 53 | 54 | std::string toString() const 55 | { 56 | std::stringstream s; 57 | s << file << ":" << line << ": " << function << "()"; 58 | return s.str(); 59 | } 60 | 61 | }; 62 | 63 | } // namespace internal 64 | } // namespace kindr 65 | 66 | inline std::ostream & operator<<(std::ostream & out, const kindr::internal::source_file_pos & sfp) 67 | { 68 | out << sfp.file << ":" << std::to_string(sfp.line) << ": " << sfp.function << "()"; 69 | return out; 70 | } 71 | 72 | 73 | #define KINDR_SOURCE_FILE_POS kindr::internal::source_file_pos(__FUNCTION__,__FILE__,__LINE__) 74 | 75 | #endif 76 | 77 | -------------------------------------------------------------------------------- /include/kindr/math/LinearAlgebra.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 20 | * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 21 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 22 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | #pragma once 29 | 30 | #include 31 | 32 | namespace kindr { 33 | 34 | 35 | /*! 36 | * \brief Gets a skew-symmetric matrix from a (column) vector 37 | * \param vec 3x1-matrix (column vector) 38 | * \return skew 3x3-matrix 39 | */ 40 | template 41 | inline static Eigen::Matrix getSkewMatrixFromVector(const Eigen::Matrix& vec) { 42 | Eigen::Matrix mat; 43 | mat << 0, -vec(2), vec(1), vec(2), 0, -vec(0), -vec(1), vec(0), 0; 44 | return mat; 45 | } 46 | 47 | /*! 48 | * \brief Gets a 3x1 vector from a skew-symmetric matrix 49 | * \param matrix 3x3-matrix 50 | * \return column vector (3x1-matrix) 51 | */ 52 | template 53 | inline static Eigen::Matrix getVectorFromSkewMatrix(const Eigen::Matrix& matrix) { 54 | return Eigen::Matrix (matrix(2,1), matrix(0,2), matrix(1,0)); 55 | } 56 | 57 | /*! 58 | * \brief Computes the Moore–Penrose pseudoinverse 59 | * info: http://eigen.tuxfamily.org/bz/show_bug.cgi?id=257 60 | * \param a: Matrix to invert 61 | * \param result: Result is written here 62 | * \param epsilon: Numerical precision (for example 1e-6) 63 | * \return true if successful 64 | */ 65 | template 66 | bool static pseudoInverse(const _Matrix_TypeA_ &a, _Matrix_TypeB_ &result, 67 | double epsilon = std::numeric_limits::epsilon()) 68 | { 69 | // Shorthands 70 | constexpr auto rowsA = static_cast(_Matrix_TypeA_::RowsAtCompileTime); 71 | constexpr auto colsA = static_cast(_Matrix_TypeA_::ColsAtCompileTime); 72 | constexpr auto rowsB = static_cast(_Matrix_TypeB_::RowsAtCompileTime); 73 | constexpr auto colsB = static_cast(_Matrix_TypeB_::ColsAtCompileTime); 74 | 75 | // Assert wrong matrix types 76 | static_assert(std::is_same::value, 77 | "[kindr::pseudoInverse] Matrices must be of the same Scalar type!"); 78 | static_assert(rowsA == colsB && colsA == rowsB, "[kindr::pseudoInverse] Result type has wrong size!"); 79 | 80 | // If one dimension is dynamic, compute everything as dynamic size 81 | constexpr auto m = Eigen::JacobiSVD< _Matrix_TypeA_ >::DiagSizeAtCompileTime; 82 | 83 | // JacobiSVD needs to be computed on dynamic size if we need ComputeThinU, ComputeThinV, see: 84 | // https://eigen.tuxfamily.org/dox/classEigen_1_1JacobiSVD.html 85 | Eigen::JacobiSVD< Eigen::Matrix > svd(a, Eigen::ComputeThinU | Eigen::ComputeThinV); 86 | 87 | typename _Matrix_TypeA_::Scalar tolerance = 88 | epsilon * std::max(a.cols(), a.rows()) * svd.singularValues().array().abs().maxCoeff(); 89 | 90 | // Sigma for ThinU and ThinV 91 | Eigen::Matrix sigmaThin = Eigen::Matrix( 92 | (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0)).asDiagonal(); 93 | 94 | result = svd.matrixV() * sigmaThin * svd.matrixU().transpose(); 95 | 96 | return true; 97 | } 98 | 99 | 100 | } // end namespace kindr 101 | -------------------------------------------------------------------------------- /include/kindr/phys_quant/PhysicalType.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 20 | * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 21 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 22 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | #ifndef KINDR_PHYS_QUANT_PHYSICALTYPE_HPP_ 29 | #define KINDR_PHYS_QUANT_PHYSICALTYPE_HPP_ 30 | 31 | namespace kindr { 32 | 33 | /*! \class PhysicalType 34 | * \brief Physical type of a vector. 35 | * 36 | * This enum class contains all possible physical types a vector can have in kindr. 37 | * \ingroup vectors 38 | */ 39 | enum class PhysicalType { 40 | Typeless, // 0 41 | 42 | Time, // 1 43 | Mass, // 2 44 | Inertia, // 3 45 | Power, // 4 46 | Energy, // 5 47 | 48 | Jerk, // 6 49 | Acceleration, // 7 50 | Velocity, // 8 51 | Position, // 9 52 | Force, // 10 53 | Momentum, // 11 54 | 55 | AngularJerk, // 12 56 | AngularAcceleration, // 13 57 | AngularVelocity, // 14 58 | Angle, // 15 59 | Torque, // 16 60 | AngularMomentum // 17 61 | }; 62 | 63 | 64 | } // namespace kindr 65 | 66 | 67 | 68 | 69 | 70 | #endif /* KINDR_PHYS_QUANT_PHYSICALTYPE_HPP_ */ 71 | -------------------------------------------------------------------------------- /include/kindr/phys_quant/WrenchBase.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 20 | * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 21 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 22 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * WrenchBase.hpp 30 | * 31 | * Created on: Jan 31, 2017 32 | * Author: Christian Gehring 33 | */ 34 | 35 | #pragma once 36 | 37 | namespace kindr { 38 | 39 | template 40 | class WrenchBase { 41 | public: 42 | /*! \brief Default constructor. 43 | * 44 | * Creates a wrench with all values set to zero. 45 | */ 46 | WrenchBase() = default; 47 | 48 | /*! \brief Constructor from derived. 49 | * This constructor has been deleted because the abstract class does not contain any data. 50 | */ 51 | WrenchBase(const Derived_&) = delete; // on purpose!! 52 | 53 | /*! \brief Gets the derived. 54 | * 55 | * (only for advanced users) 56 | * \returns the derived time derivative of a pose 57 | */ 58 | operator Derived_& () { 59 | return static_cast(*this); 60 | } 61 | 62 | /*! \brief Gets the derived. 63 | * 64 | * (only for advanced users) 65 | * \returns the derived time derivative of a pose 66 | */ 67 | operator const Derived_& () const { 68 | return static_cast(*this); 69 | } 70 | 71 | /*! \brief Gets the derived. 72 | * 73 | * (only for advanced users) 74 | * \returns the derived time derivative of a pose 75 | */ 76 | const Derived_& derived() const { 77 | return static_cast(*this); 78 | } 79 | 80 | /*! \brief Sets wrench to zero 81 | * \returns reference 82 | */ 83 | Derived_& setZero(); 84 | }; 85 | 86 | } // namespace 87 | -------------------------------------------------------------------------------- /include/kindr/poses/Pose.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 20 | * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 21 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 22 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #pragma once 30 | 31 | #include "kindr/poses/HomogeneousTransformation.hpp" 32 | 33 | namespace kindr { 34 | 35 | template 36 | class HomogeneousTransformation; 37 | 38 | 39 | namespace internal { 40 | 41 | /*! \brief Multiplication of two rotations with the same parameterization 42 | */ 43 | template 44 | class MultiplicationTraits, PoseBase > { 45 | public: 46 | typedef typename Left_::Position Position; 47 | typedef typename Left_::Rotation Rotation; 48 | typedef typename Left_::Scalar Scalar; 49 | typedef HomogeneousTransformation HomTrans; 50 | public: 51 | //! Default multiplication of rotations converts the representations of the rotations to rotation quaternions and multiplies them 52 | inline static Left_ mult(const PoseBase& lhs, const PoseBase& rhs) { 53 | const Position position = lhs.derived().getPosition()+lhs.derived().getRotation().rotate(rhs.derived().getPosition()); 54 | const Rotation rotation = lhs.derived().getRotation()*rhs.derived().getRotation(); 55 | return Left_(HomTrans(position, rotation)); 56 | } 57 | }; 58 | 59 | ///*! \brief Multiplication of two rotations with the same parameterization 60 | // */ 61 | //template 62 | //class MultiplicationTraits, HomogeneousTransformation > { 63 | // public: 64 | // //! Default multiplication of rotations converts the representations of the rotations to rotation quaternions and multiplies them 65 | // inline static HomogeneousTransformation mult(const HomogeneousTransformation& lhs, const HomogeneousTransformation& rhs) { 66 | // const typename HomogeneousTransformation::Position position = lhs.getPosition()+lhs.getRotation().rotate(rhs.getPosition()); 67 | // const typename HomogeneousTransformation::Rotation rotation = lhs.getRotation()*rhs.getRotation(); 68 | // return HomogeneousTransformation(position, rotation); 69 | // } 70 | //}; 71 | 72 | ///*! \brief Multiplication of two rotations with the same parameterization 73 | // */ 74 | //template 75 | //class MultiplicationTraits, RotationBase > { 76 | // public: 77 | // //! Default multiplication of rotations converts the representations of the rotations to rotation quaternions and multiplies them 78 | // inline static LeftAndRight_ mult(const RotationBase& lhs, const RotationBase& rhs) { 79 | // return LeftAndRight_(RotationQuaternion( 80 | // (RotationQuaternion(lhs.derived())).toImplementation() * 81 | // (RotationQuaternion(rhs.derived())).toImplementation() 82 | // )); 83 | // 84 | // } 85 | //}; 86 | 87 | /*! \brief Compare two poses. 88 | */ 89 | template 90 | class ComparisonTraits, PoseBase>{ 91 | public: 92 | inline static bool isEqual(const PoseBase& lhs, const PoseBase& rhs) { 93 | return lhs.derived().getPosition() == rhs.derived().getPosition() && 94 | lhs.derived().getRotation() == rhs.derived().getRotation(); 95 | } 96 | }; 97 | 98 | 99 | } // namespace internal 100 | } // namespace kindr 101 | -------------------------------------------------------------------------------- /include/kindr/poses/PoseBase.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 20 | * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 21 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 22 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #pragma once 30 | 31 | 32 | #include "kindr/common/common.hpp" 33 | 34 | 35 | namespace kindr { 36 | //! Generic pose interface 37 | /*! \ingroup poses 38 | */ 39 | 40 | namespace internal { 41 | 42 | /*! \class TransformationTraits 43 | * \brief Transformation trait to implement transformations of positions. 44 | * 45 | * (only for advanced users) 46 | */ 47 | template 48 | class TransformationTraits { 49 | public: 50 | // inline static Position transform(const Pose& pose, const Position& position); 51 | // inline static Position inverseTransform(const Pose& pose, const Position& position); 52 | }; 53 | 54 | /*! \class get_position 55 | * \brief This class provides the type of the position. 56 | * 57 | * (only for advanced users) 58 | */ 59 | template 60 | class get_position { 61 | public: 62 | // typedef typename positions::Position3D Position; 63 | }; 64 | 65 | } // namespace internal 66 | 67 | 68 | 69 | /*! \class PoseBase 70 | * \brief Base class that defines the interface of a pose of a rigid body. 71 | * 72 | * This class defines the interface of a pose of a rigid body. 73 | * A pose of a rigid body describes the position and orientation of the rigid body in space. 74 | * The position and orientation are described by position coordinates and a rotation, respectively. 75 | * \tparam Derived_ the derived class that should implement the pose. 76 | * \ingroup poses 77 | */ 78 | template 79 | class PoseBase { 80 | public: 81 | /*! \brief Default constructor. 82 | * 83 | * Creates a pose with all position coordinates set to zero and an identity orientation. 84 | */ 85 | PoseBase() = default; 86 | 87 | /*! \brief Constructor from derived pose. 88 | * This constructor has been deleted because the abstract class does not contain any data. 89 | */ 90 | PoseBase(const Derived_&) = delete; // on purpose!! 91 | 92 | /*! \brief Gets the derived pose. 93 | * 94 | * (only for advanced users) 95 | * \returns the derived pose 96 | */ 97 | operator Derived_& () { 98 | return static_cast(*this); 99 | } 100 | 101 | /*! \brief Gets the derived pose. 102 | * 103 | * (only for advanced users) 104 | * \returns the derived pose 105 | */ 106 | operator const Derived_& () const { 107 | return static_cast(*this); 108 | } 109 | 110 | /*! \brief Gets the derived pose. 111 | * 112 | * (only for advanced users) 113 | * \returns the derived pose 114 | */ 115 | const Derived_& derived() const { 116 | return static_cast(*this); 117 | } 118 | 119 | 120 | /*! \brief Transforms a position. 121 | * \returns the transformed position 122 | */ 123 | typename internal::get_position::Position transform(const typename internal::get_position::Position& position) const { 124 | return internal::TransformationTraits::transform(this->derived(), position); 125 | } 126 | 127 | /*! \brief Transforms a position in reverse 128 | * \returns the transformed position 129 | */ 130 | typename internal::get_position::Position inverseTransform(const typename internal::get_position::Position& position) const { 131 | return internal::TransformationTraits::inverseTransform(this->derived(), position); 132 | } 133 | 134 | /*! \brief Concatenates two transformations. 135 | * \returns the concatenation of two transformations 136 | */ 137 | template 138 | Derived_ operator *(const PoseBase& other) const { 139 | return internal::MultiplicationTraits,PoseBase>::mult(this->derived(), other.derived()); // todo: 1. ok? 2. may be optimized 140 | } 141 | 142 | /*! \brief Compares two poses for equality. 143 | * \returns true if the poses are exactly equal 144 | */ 145 | template 146 | bool operator ==(const PoseBase& other) const { 147 | return internal::ComparisonTraits,PoseBase>::isEqual(*this, other); 148 | } 149 | 150 | /*! \brief Compares two poses for inequality. 151 | * \returns true if the poses are not exactly equal 152 | */ 153 | template 154 | bool operator !=(const PoseBase& other) const { 155 | return !(*this == other); 156 | } 157 | 158 | /*! \brief Sets the pose to identity 159 | * \returns reference 160 | */ 161 | Derived_& setIdentity(); 162 | }; 163 | 164 | 165 | } // namespace kindr 166 | 167 | -------------------------------------------------------------------------------- /include/kindr/poses/PoseDiff.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 20 | * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 21 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 22 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #pragma once 30 | 31 | #include "kindr/poses/Twist.hpp" 32 | -------------------------------------------------------------------------------- /include/kindr/poses/PoseDiffBase.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 20 | * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 21 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 22 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #pragma once 30 | 31 | namespace kindr { 32 | 33 | /*! \class PoseDiffBase 34 | * \brief Base class that defines the interface of the time derivative of a pose of a rigid body. 35 | * 36 | * This class defines the interface of the time derivative of a pose of a rigid body. 37 | * The time derivative of a pose of a rigid body describes the linear and angular velocities of the rigid body in space. 38 | * \tparam Derived_ the derived class that should implement the time derivative of the pose. 39 | * \ingroup poses 40 | */ 41 | template 42 | class PoseDiffBase { 43 | public: 44 | /*! \brief Default constructor. 45 | * 46 | * Creates a time derivative of a pose with all derivatives set to zero. 47 | */ 48 | PoseDiffBase() = default; 49 | 50 | /*! \brief Constructor from derived time derivative of a pose. 51 | * This constructor has been deleted because the abstract class does not contain any data. 52 | */ 53 | PoseDiffBase(const Derived_&) = delete; // on purpose!! 54 | 55 | /*! \brief Gets the derived time derivative of a pose. 56 | * 57 | * (only for advanced users) 58 | * \returns the derived time derivative of a pose 59 | */ 60 | operator Derived_& () { 61 | return static_cast(*this); 62 | } 63 | 64 | /*! \brief Gets the derived time derivative of a pose. 65 | * 66 | * (only for advanced users) 67 | * \returns the derived time derivative of a pose 68 | */ 69 | operator const Derived_& () const { 70 | return static_cast(*this); 71 | } 72 | 73 | /*! \brief Gets the derived time derivative of a pose. 74 | * 75 | * (only for advanced users) 76 | * \returns the derived time derivative of a pose 77 | */ 78 | const Derived_& derived() const { 79 | return static_cast(*this); 80 | } 81 | 82 | /*! \brief Sets twist to zero 83 | * \returns reference 84 | */ 85 | Derived_& setZero(); 86 | }; 87 | 88 | 89 | } // namespace kindr 90 | -------------------------------------------------------------------------------- /include/kindr/rotations/Rotation.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 20 | * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 21 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 22 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #pragma once 30 | 31 | #include "kindr/rotations/RotationImpl.hpp" 32 | #include "kindr/rotations/AngleAxis.hpp" 33 | #include "kindr/rotations/RotationVector.hpp" 34 | #include "kindr/rotations/RotationQuaternion.hpp" 35 | #include "kindr/rotations/RotationMatrix.hpp" 36 | #include "kindr/rotations/EulerAnglesZyx.hpp" 37 | #include "kindr/rotations/EulerAnglesXyz.hpp" 38 | 39 | 40 | -------------------------------------------------------------------------------- /include/kindr/rotations/RotationConversion.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 20 | * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 21 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 22 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | /* 30 | * convention.hpp 31 | * 32 | * Created on: Jun 16, 2016 33 | * Author: Christian Gehring 34 | */ 35 | 36 | #pragma once 37 | 38 | #include 39 | 40 | namespace kindr { 41 | 42 | /*! Implement the conversions from the kindr convention to the other convention. 43 | * Specialize the conversion traits 44 | */ 45 | template 46 | class RotationConversion { 47 | public: 48 | inline static void convertToOtherRotation(OtherRotation_& otherRotation, const kindr::RotationQuaternion& quaternionIn) { 49 | // Implement rotation = f(quaternionIn); 50 | static_assert(sizeof(OtherRotation_) == -1, "You need to implement the method convertToOtherRotation!"); 51 | } 52 | 53 | inline static void convertToKindr(kindr::RotationQuaternion& quaternion, const OtherRotation_& otherRotation) { 54 | static_assert(sizeof(OtherRotation_) == -1, "You need to implement the method convertToKindr!"); 55 | } 56 | 57 | inline static void convertToOtherVelocityVector(OtherVelocity_& otherVelocity, const OtherRotation_& rotation, const Eigen::Matrix& velocityIn) { 58 | // Implement otherVelocity = g(velocityIn, rotation); 59 | static_assert(sizeof(OtherRotation_) == -1, "You need to implement the method convertToOtherVelocityVector!"); 60 | } 61 | 62 | inline static void getRotationMatrixFromRotation(Eigen::Matrix& rotationMatrix, const OtherRotation_& rotation) { 63 | // Implement rotationMatrix = C(rotation); 64 | static_assert(sizeof(OtherRotation_) == -1, "You need to implement the method getRotationMatrixFromRotation!"); 65 | } 66 | 67 | inline static void concatenate(OtherRotation_& result, const OtherRotation_& rot1, OtherRotation_& rot2) { 68 | static_assert(sizeof(OtherRotation_) == -1, "You need to implement the method concatenate!"); 69 | } 70 | 71 | inline static void rotateVector(Eigen::Matrix& A_r, const OtherRotation_& rotationBToA, const Eigen::Matrix& B_r) { 72 | // Implement A_r = rotationBtoA.rotate(A_r); 73 | static_assert(sizeof(OtherRotation_) == -1, "You need to implement the method rotateVector!"); 74 | } 75 | 76 | inline static void boxPlus(OtherRotation_& result, const OtherRotation_& rotation, const OtherVelocity_& velocity) { 77 | // Implement result = rotation.boxPlus(vector); 78 | static_assert(sizeof(OtherRotation_) == -1, "You need to implement the method boxPlus!"); 79 | } 80 | 81 | inline static bool testRotation(const OtherRotation_& expected, const OtherRotation_& actual) { 82 | // Implement EXPECT_NEAR(expected, actual, 1.0e-6); 83 | static_assert(sizeof(OtherRotation_) == -1, "You need to implement the method testRotation!"); 84 | return false; 85 | } 86 | }; 87 | 88 | } // namespace 89 | -------------------------------------------------------------------------------- /include/kindr/rotations/RotationDiff.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 20 | * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 21 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 22 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #pragma once 30 | 31 | #include "kindr/math/LinearAlgebra.hpp" 32 | #include "kindr/rotations/RotationBase.hpp" 33 | 34 | namespace kindr { 35 | 36 | 37 | template 38 | class LocalAngularVelocity; 39 | 40 | template 41 | class GlobalAngularVelocity; 42 | 43 | template 44 | class RotationDiffBase; 45 | 46 | template 47 | class RotationQuaternion; 48 | 49 | template 50 | class RotationQuaternionDiff; 51 | 52 | template 53 | class RotationMatrixDiff; 54 | 55 | template 56 | class EulerAnglesZyxDiff; 57 | 58 | template 59 | class EulerAnglesXyzDiff; 60 | 61 | 62 | template 63 | inline static Right_ operator*(const RotationBase& lhs, const RotationDiffBase& rhs) { 64 | return Right_(RotationQuaternionDiff( 65 | (RotationQuaternion(lhs.derived())).toImplementation() * 66 | (RotationQuaternionDiff(rhs.derived())).toImplementation() 67 | )); 68 | } 69 | 70 | template 71 | inline static Left_ operator*(const RotationDiffBase& lhs, const RotationBase& rhs) { 72 | return Left_(RotationQuaternionDiff( 73 | (RotationQuaternionDiff(lhs.derived())).toImplementation() * 74 | (RotationQuaternion(rhs.derived())).toImplementation() 75 | )); 76 | } 77 | 78 | /*! 79 | * \brief Gets the 3x3 Jacobian of the exponential map. 80 | * \param vector 3x1-matrix 81 | * \return matrix (3x3-matrix) 82 | */ 83 | template 84 | inline static Eigen::Matrix getJacobianOfExponentialMap(const Eigen::Matrix& vector) { 85 | const PrimType_ norm = vector.norm(); 86 | const Eigen::Matrix skewMatrix = getSkewMatrixFromVector(vector); 87 | if (norm < 1.0e-4) { 88 | return Eigen::Matrix::Identity() + 0.5*skewMatrix; 89 | } 90 | return Eigen::Matrix::Identity() + (PrimType_(1.0) - cos(norm))/(norm*norm)*skewMatrix + (norm - sin(norm))/(norm*norm*norm)*(skewMatrix*skewMatrix); 91 | } 92 | 93 | 94 | } // namespace kindr 95 | 96 | 97 | #include "kindr/rotations/LocalAngularVelocity.hpp" 98 | #include "kindr/rotations/GlobalAngularVelocity.hpp" 99 | #include "kindr/rotations/RotationQuaternionDiff.hpp" 100 | #include "kindr/rotations/RotationMatrixDiff.hpp" 101 | #include "kindr/rotations/EulerAnglesZyxDiff.hpp" 102 | #include "kindr/rotations/EulerAnglesXyzDiff.hpp" 103 | 104 | 105 | -------------------------------------------------------------------------------- /kindrConfig.cmake.in: -------------------------------------------------------------------------------- 1 | # - Config file for the kindr package 2 | # It defines the following variables 3 | # kindr_INCLUDE_DIRS - include directories for kindr 4 | 5 | # Compute paths 6 | get_filename_component(kindr_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) 7 | set(kindr_INCLUDE_DIRS "@kindr_include_dirs@") 8 | 9 | # This causes catkin_simple to link against these libraries 10 | set(kindr_FOUND_CATKIN_PROJECT true) 11 | -------------------------------------------------------------------------------- /matlab/core/boxMinus.m: -------------------------------------------------------------------------------- 1 | function delta = boxMinus(C1, C2) 2 | % BOXMINUS(C1, C2) extracts the perturbation delta from two rotation 3 | % matrices C1 and C2. 4 | % 5 | % Author(s): Dario Bellicoso 6 | 7 | delta = unskew(logm(C1*C2')); 8 | 9 | end -------------------------------------------------------------------------------- /matlab/core/boxPlus.m: -------------------------------------------------------------------------------- 1 | function Cd = boxPlus(C, delta) 2 | % BOXPLUS(C, delta) performs a global perturbation of a rotation matrix C 3 | % with a rotational vector delta. 4 | % 5 | % Author(s): Dario Bellicoso 6 | 7 | Cd = expm(skew(delta))*C; 8 | 9 | end -------------------------------------------------------------------------------- /matlab/core/getAngleAxisFromRotationMatrix.m: -------------------------------------------------------------------------------- 1 | function [th, n] = getAngleAxisFromRotationMatrix(C) 2 | % GETANGLEAXISFROMROTATIONMATRIX(C) extracts an angle and an axis from a 3 | % rotation matrix C. 4 | % 5 | % Author(s): Dario Bellicoso 6 | 7 | th = acos(0.5*(C(1,1)+C(2,2)+C(3,3)-1)); 8 | n = 1/(2*sin(th))*[C(3,2)-C(2,3); 9 | C(1,3)-C(3,1); 10 | C(2,1)-C(1,2)]; 11 | 12 | end -------------------------------------------------------------------------------- /matlab/core/getEulAngXYZFromRotationMatrix.m: -------------------------------------------------------------------------------- 1 | function ph = getEulAngXYZFromRotationMatrix(C) 2 | % GETEULANGXYZFROMROTATIONMATRIX(C) extracts XYZ Euler angles from a 3 | % rotation matrix. 4 | % 5 | % Author(s): Dario Bellicoso 6 | 7 | x = atan2(-C(2,3),C(3,3)); 8 | y = atan2(C(1,3), sqrt(C(1,1)^2+C(1,2)^2)); 9 | z = atan2(-C(1,2),C(1,1)); 10 | 11 | ph = [x y z]'; 12 | -------------------------------------------------------------------------------- /matlab/core/getEulAngZXZFromRotationMatrix.m: -------------------------------------------------------------------------------- 1 | function ph = getEulAngZXZFromRotationMatrix(C) 2 | % GETEULANGZYXFROMROTATIONMATRIX(C) extracts ZXZ Euler angles from a 3 | % rotation matrix. 4 | % 5 | % Author(s): Dario Bellicoso 6 | 7 | z1 = atan2(C(1,3),-C(2,3)); 8 | x = atan2(sqrt(C(1,3)^2+C(2,3)^2), C(3,3)); 9 | z2 = atan2(C(3,1),C(3,2)); 10 | 11 | ph = [z1 x z2]'; 12 | -------------------------------------------------------------------------------- /matlab/core/getEulAngZYXFromRotationMatrix.m: -------------------------------------------------------------------------------- 1 | function ph = getEulAngZYXFromRotationMatrix(C) 2 | % GETEULANGZYXFROMROTATIONMATRIX(C) extracts ZYX Euler angles from a 3 | % rotation matrix. 4 | % 5 | % Author(s): Dario Bellicoso 6 | 7 | z = atan2(C(2,1),C(1,1)); 8 | y = atan2(-C(3,1), sqrt(C(3,2)^2+C(3,3)^2)); 9 | x = atan2(C(3,2),C(3,3)); 10 | 11 | ph = [z y x]'; 12 | -------------------------------------------------------------------------------- /matlab/core/getEulAngZYZFromRotationMatrix.m: -------------------------------------------------------------------------------- 1 | function ph = getEulAngZYZFromRotationMatrix(C) 2 | % GETEULANGZYXFROMROTATIONMATRIX(C) extracts ZYZ Euler angles from a 3 | % rotation matrix. 4 | % 5 | % Author(s): Dario Bellicoso 6 | 7 | z1 = atan2(C(2,3),C(1,3)); 8 | y = atan2(sqrt(C(1,3)^2+C(2,3)^2), C(3,3)); 9 | z2 = atan2(C(3,2),-C(3,1)); 10 | 11 | ph = [z1 y z2]'; 12 | -------------------------------------------------------------------------------- /matlab/core/getMapEulAngZXZDiffToAngVelInWorldFrame.m: -------------------------------------------------------------------------------- 1 | function E = getMapEulAngZXZDiffToAngVelInWorldFrame(angles) 2 | % GETMAPEULANGZXZDIFFTOANGVELINWORLDFRAME(angles) returns the 3x3 matrix 3 | % that maps the time derivative of ZXZ Euler angles to angular velocity in 4 | % world frame. 5 | % 6 | % Author(s): Dario Bellicoso 7 | 8 | z1 = angles(1); 9 | x = angles(2); 10 | 11 | % The mapping is computed as: 12 | % [I_w_IBx] [dz1] 13 | % [I_w_IBy] = E([z1,x,z2]') * [dx] 14 | % [I_w_IBz] [dz2] 15 | E = [0 cos(z1) sin(x)*sin(z1); 16 | 0 sin(z1) -cos(z1)*sin(x) 17 | 1 0 cos(x)]; 18 | 19 | if (isa(angles, 'sym')) 20 | E = simplify(E); 21 | end 22 | 23 | end -------------------------------------------------------------------------------- /matlab/core/getMapEulAngZYXDDiffToAngAccInBaseFrame.m: -------------------------------------------------------------------------------- 1 | function H = getMapEulAngZYXDDiffToAngAccInBaseFrame(angles, dAngles, I_w_IB) 2 | % GETMAPEULANGZYXDDIFFTOANGACCINBASEFRAME(angles, dAngles) returns the 3x3 3 | % matrix that maps the time derivative of ZYX Euler angles to angular 4 | % acceleration in base frame. 5 | % 6 | % Author(s): Dario Bellicoso 7 | 8 | z = angles(1); 9 | y = angles(2); 10 | 11 | dz = dAngles(1); 12 | dy = dAngles(2); 13 | 14 | % The mapping is computed as: 15 | % [B_w_IBx] [dz] 16 | % [B_w_IBy] = ( dR_IB/dt * E([z,y,x]') + R_IB * dE([z,y,x]')* [dy] 17 | % [B_w_IBz] [dx] 18 | % 19 | % The time derivative is then obtained by differentiating this mapping 20 | % w.r.t. time. 21 | R_IB = mapEulerAnglesZYXToRotationMatrix(angles); 22 | dR_IB = skew(I_w_IB)*R_IB; 23 | 24 | E = getMapEulAngZYXDiffToAngVelInWorldFrame(angles); 25 | 26 | dE = [0 -dz*cos(z) -dy*cos(z)*sin(y)-dz*cos(y)*sin(z); 27 | 0 -dz*sin(z) dz*cos(y)*cos(z)-dy*sin(y)*sin(z); 28 | 0 0 -dy*cos(y)]; 29 | 30 | 31 | H = dR_IB*E + R_IB*dE; 32 | 33 | if (isa(angles, 'sym')) 34 | H = simplify(H); 35 | end 36 | 37 | end -------------------------------------------------------------------------------- /matlab/core/getMapEulAngZYXDDiffToAngAccInWorldFrame.m: -------------------------------------------------------------------------------- 1 | function E = getMapEulAngZYXDDiffToAngAccInWorldFrame(angles, dAngles) 2 | % GETMAPEULANGZYXDDIFFTOANGACCINWORLDFRAME(angles, dAngles) returns the 3x3 3 | % matrix that maps the time derivative of ZYX Euler angles to angular 4 | % acceleration in world frame. 5 | % 6 | % Author(s): Dario Bellicoso 7 | 8 | z = angles(1); 9 | y = angles(2); 10 | 11 | dz = dAngles(1); 12 | dy = dAngles(2); 13 | 14 | % The mapping is computed as: 15 | % [wx] [dz] 16 | % [wy] = E([z,y,x]') * [dy] 17 | % [wz] [dx] 18 | % 19 | % The time derivative is then obtained by differentiating this mapping 20 | % w.r.t. time. 21 | E = [0 -dz*cos(z) -dy*cos(z)*sin(y)-dz*cos(y)*sin(z); 22 | 0 -dz*sin(z) dz*cos(y)*cos(z)-dy*sin(y)*sin(z); 23 | 0 0 -dy*cos(y)]; 24 | 25 | 26 | if (isa(angles, 'sym')) 27 | E = simplify(E); 28 | end 29 | 30 | end -------------------------------------------------------------------------------- /matlab/core/getMapEulAngZYXDiffToAngVelInBaseFrame.m: -------------------------------------------------------------------------------- 1 | function H = getMapEulAngZYXDiffToAngVelInBaseFrame(angles) 2 | % GETMAPEULANGZYXDIFFTOANGVELINBASEFRAME(angles) returns the 3x3 matrix 3 | % that maps the time derivative of ZYX Euler angles to angular velocity in 4 | % base frame. 5 | % 6 | % Author(s): Dario Bellicoso 7 | 8 | z = angles(1); 9 | y = angles(2); 10 | 11 | % The mapping is computed as: 12 | % [B_w_IBx] [dz] 13 | % [B_w_IBy] = R_BI * E([z,y,x]') * [dy] 14 | % [B_w_IBz] [dx] 15 | % 16 | % H = R_BI * E([z,y,x]') 17 | % 18 | R_IB = mapEulerAnglesZYXToRotationMatrix(angles); 19 | H = R_IB' * [0 -sin(z) cos(y)*cos(z); 20 | 0 cos(z) cos(y)*sin(z) 21 | 1 0 -sin(y)]; 22 | 23 | if (isa(angles, 'sym')) 24 | H = simplify(H); 25 | end 26 | 27 | end -------------------------------------------------------------------------------- /matlab/core/getMapEulAngZYXDiffToAngVelInWorldFrame.m: -------------------------------------------------------------------------------- 1 | function E = getMapEulAngZYXDiffToAngVelInWorldFrame(angles) 2 | % GETMAPEULANGZYXDIFFTOANGVELINWORLDFRAME(angles) returns the 3x3 matrix 3 | % that maps the time derivative of ZYX Euler angles to angular velocity in 4 | % world frame. 5 | % 6 | % Author(s): Dario Bellicoso 7 | 8 | z = angles(1); 9 | y = angles(2); 10 | 11 | % The mapping is computed as: 12 | % [I_w_IBx] [dz] 13 | % [I_w_IBy] = E([z,y,x]') * [dy] 14 | % [I_w_IBz] [dx] 15 | E = [0 -sin(z) cos(y)*cos(z); 16 | 0 cos(z) cos(y)*sin(z) 17 | 1 0 -sin(y)]; 18 | 19 | if (isa(angles, 'sym')) 20 | E = simplify(E); 21 | end 22 | 23 | end -------------------------------------------------------------------------------- /matlab/core/getMapEulAngZYZDiffToAngVelInWorldFrame.m: -------------------------------------------------------------------------------- 1 | function E = getMapEulAngZYZDiffToAngVelInWorldFrame(angles) 2 | % GETMAPEULANGZYZDIFFTOANGVELINWORLDFRAME(angles) returns the 3x3 matrix 3 | % that maps the time derivative of ZYZ Euler angles to angular velocity in 4 | % world frame. 5 | % 6 | % Author(s): Dario Bellicoso 7 | 8 | z = angles(1); 9 | y = angles(2); 10 | 11 | % The mapping is computed as: 12 | % [I_w_IBx] [dz] 13 | % [I_w_IBy] = E([z1,y,z2]') * [dy] 14 | % [I_w_IBz] [dx] 15 | E = [0 -sin(z) cos(y)*cos(z); 16 | 0 cos(z) cos(y)*sin(z) 17 | 1 0 -sin(y)]; 18 | 19 | if (isa(angles, 'sym')) 20 | E = simplify(E); 21 | end 22 | 23 | end -------------------------------------------------------------------------------- /matlab/core/getRotationMatrixX.m: -------------------------------------------------------------------------------- 1 | function R = getRotationMatrixX(gamma) 2 | 3 | c = cos(gamma); 4 | s = sin(gamma); 5 | 6 | if ~isa(gamma,'sym') 7 | if abs( c - round(c) ) < eps 8 | c = round(c); 9 | end 10 | 11 | if abs( s - round(s) ) < eps 12 | s = round(s); 13 | end 14 | end 15 | 16 | R = [1 0 0; 17 | 0 c -s; 18 | 0 s c]; 19 | 20 | end -------------------------------------------------------------------------------- /matlab/core/getRotationMatrixY.m: -------------------------------------------------------------------------------- 1 | function R = getRotationMatrixY(beta) 2 | 3 | c = cos(beta); 4 | s = sin(beta); 5 | 6 | if ~isa(beta,'sym') 7 | if abs( c - round(c) ) < eps 8 | c = round(c); 9 | end 10 | 11 | if abs( s - round(s) ) < eps 12 | s = round(s); 13 | end 14 | end 15 | 16 | R = [c 0 s; 17 | 0 1 0; 18 | -s 0 c]; 19 | 20 | end -------------------------------------------------------------------------------- /matlab/core/getRotationMatrixZ.m: -------------------------------------------------------------------------------- 1 | function R = getRotationMatrixZ(alpha) 2 | 3 | c = cos(alpha); 4 | s = sin(alpha); 5 | 6 | if ~isa(alpha,'sym') 7 | if abs( c - round(c) ) < eps 8 | c = round(c); 9 | end 10 | 11 | if abs( s - round(s) ) < eps 12 | s = round(s); 13 | end 14 | end 15 | 16 | R = [c -s 0; 17 | s c 0; 18 | 0 0 1]; 19 | 20 | end -------------------------------------------------------------------------------- /matlab/core/getTimeDerivative.m: -------------------------------------------------------------------------------- 1 | function dAdt = getTimeDerivative(A, x, dx) 2 | % GETTIMEDERIVATIVE(A, x, dx) computes the time derivative of A(x(t)) 3 | % 4 | % Author(s): Dario Bellicoso 5 | 6 | 7 | [m,n] = size(A); 8 | 9 | if isa(A, 'sym') 10 | dAdt = sym(zeros(m,n)); 11 | else 12 | dAdt = zeros(m,n); 13 | end 14 | 15 | % Make dx a column vector 16 | dx = dx(:); 17 | 18 | for k=1:m 19 | for h=1:n 20 | dAdt(k,h) = jacobian(A(k,h), x)*dx; 21 | end 22 | end 23 | 24 | 25 | if isa(A, 'sym') 26 | dAdt = simplify(dAdt); 27 | end 28 | 29 | end -------------------------------------------------------------------------------- /matlab/core/isRotationMatrix.m: -------------------------------------------------------------------------------- 1 | function [res, str] = isRotationMatrix(C) 2 | % ISROTATIONMATRIX(C) checks if the 3x3 matrix C is an element of SO(3) 3 | % 4 | % Author(s): Dario Bellicoso 5 | 6 | % Initialize output 7 | res = true; 8 | str = ' '; 9 | 10 | % Check size 11 | [m,n] = size(C); 12 | if (m~=3 || n~=3) 13 | res = false; 14 | str = [str 'Checking rotation matrix: input is not 3x3.']; 15 | end 16 | 17 | % Check determinant 18 | detC = det(C); 19 | if ( (detC-1) > 1e-10) 20 | res = false; 21 | str = [str ' Determinant of C was not 1. det(C) = ' num2str(detC) '.']; 22 | end 23 | 24 | % Check orthonormality 25 | if (norm( (C*C')-eye(3)) > 1e-10) 26 | res = false; 27 | str = [str ' C is not orthonormal.']; 28 | end 29 | 30 | end -------------------------------------------------------------------------------- /matlab/core/mapAngleAxisToQuaternion.m: -------------------------------------------------------------------------------- 1 | function q = mapAngleAxisToQuaternion(th, r) 2 | % MAPANGLEAXISTOQUATERNION(th, r) maps an angle-axis parametrization to a 3 | % quaternion one, represented as q = [qw qx qy qz]' 4 | % 5 | % Author(s): Dario Bellicoso 6 | 7 | q0 = cos(th/2); 8 | qv = sin(th/2)*r(:); 9 | 10 | q = [q0; 11 | qv]; 12 | 13 | end -------------------------------------------------------------------------------- /matlab/core/mapEulerAnglesXYZToRotationMatrix.m: -------------------------------------------------------------------------------- 1 | function R = mapEulerAnglesXYZToRotationMatrix(angles) 2 | % MAPEULERANGLESXYZTOROTATIONMATRIX(angles) maps a set of Euler angles to a 3 | % rotation matrix in SO(3). The Euler angles represent a set successive 4 | % rotations around X-Y'-Z''. This is equivalent to rotating around the 5 | % fixed axes in Z-Y-X order. 6 | % 7 | % Author(s): Dario Bellicoso 8 | 9 | x = angles(1); 10 | y = angles(2); 11 | z = angles(3); 12 | 13 | R = getRotationMatrixX(x)*getRotationMatrixY(y)*getRotationMatrixZ(z); 14 | 15 | if isa(angles, 'sym') 16 | R = simplify(R); 17 | end 18 | 19 | end -------------------------------------------------------------------------------- /matlab/core/mapEulerAnglesZXZToRotationMatrix.m: -------------------------------------------------------------------------------- 1 | function R = mapEulerAnglesZXZToRotationMatrix(angles) 2 | % MAPEULERANGLESZXZTOROTATIONMATRIX(angles) maps a set of Euler angles to a 3 | % rotation matrix in SO(3). The Euler angles represent a set successive 4 | % rotations around Z-X'-Z''. 5 | % 6 | % Author(s): Dario Bellicoso 7 | 8 | z1 = angles(1); 9 | x = angles(2); 10 | z2 = angles(3); 11 | 12 | R = getRotationMatrixZ(z1)*getRotationMatrixX(x)*getRotationMatrixZ(z2); 13 | 14 | if isa(angles, 'sym') 15 | R = simplify(R); 16 | end 17 | 18 | end -------------------------------------------------------------------------------- /matlab/core/mapEulerAnglesZYXToRotationMatrix.m: -------------------------------------------------------------------------------- 1 | function R = mapEulerAnglesZYXToRotationMatrix(angles) 2 | % MAPEULERANGLESZYXTOROTATIONMATRIX(angles) maps a set of Euler angles to a 3 | % rotation matrix in SO(3). The Euler angles represent a set successive 4 | % rotations around Z-Y'-X''. This is equivalent to rotating around the 5 | % fixed axes in X-Y-Z order. 6 | % 7 | % Author(s): Dario Bellicoso 8 | 9 | z = angles(1); 10 | y = angles(2); 11 | x = angles(3); 12 | 13 | if isa(angles, 'sym') 14 | R = simplify(getRotationMatrixZ(z)*getRotationMatrixY(y)*getRotationMatrixX(x)); 15 | else 16 | R = getRotationMatrixZ(z)*getRotationMatrixY(y)*getRotationMatrixX(x); 17 | end 18 | 19 | end -------------------------------------------------------------------------------- /matlab/core/mapEulerAnglesZYZToRotationMatrix.m: -------------------------------------------------------------------------------- 1 | function R = mapEulerAnglesZYZToRotationMatrix(angles) 2 | % MAPEULERANGLESZYZTOROTATIONMATRIX(angles) maps a set of Euler angles to a 3 | % rotation matrix in SO(3). The Euler angles represent a set successive 4 | % rotations around Z-Y'-Z''. 5 | % 6 | % Author(s): Dario Bellicoso 7 | 8 | z1 = angles(1); 9 | y = angles(2); 10 | z2 = angles(3); 11 | 12 | R = getRotationMatrixZ(z1)*getRotationMatrixY(y)*getRotationMatrixZ(z2); 13 | 14 | if isa(angles, 'sym') 15 | R = simplify(R); 16 | end 17 | 18 | end -------------------------------------------------------------------------------- /matlab/core/mapQuaternionToRotationMatrix.m: -------------------------------------------------------------------------------- 1 | function C = mapQuaternionToRotationMatrix(q) 2 | % MAPQUATERNIONTOROTATIONMATRIX maps a quaternion q to SO(3) 3 | % The function MAPQUATERNIONTOROTATIONMATRIX(q) maps a quaternion 4 | % q = [qw qx qy qz]' to a rotation matrix C. 5 | % 6 | % Author(s): Dario Bellicoso 7 | 8 | q0 = q(1); 9 | qv = q(2:4); 10 | 11 | sQv = skew(qv); 12 | 13 | C = (2*q0^2-1)*eye(3) + 2*q0*sQv + 2*(qv*qv'); 14 | 15 | end -------------------------------------------------------------------------------- /matlab/core/mapRotationMatrixToQuaternion.m: -------------------------------------------------------------------------------- 1 | function q = mapRotationMatrixToQuaternion(C) 2 | % MAPROTATIONMATRIXTOQUATERNION(C) extracts a quaternion from a rotation 3 | % matrix C. 4 | % 5 | % Author(s): Dario Bellicoso 6 | 7 | [th, n] = getAngleAxisFromRotationMatrix(C); 8 | q = mapAngleAxisToQuaternion(th, n); 9 | 10 | end -------------------------------------------------------------------------------- /matlab/core/mapRotationMatrixToRotationVector.m: -------------------------------------------------------------------------------- 1 | function phi = mapRotationMatrixToRotationVector(C) 2 | % MAPROTATIONMATRIXTOROTATIONVECTOR maps a rotation matrix in SO(3) to 3 | % a rotation vector phi in so(3). 4 | % 5 | % 6 | % syntax: phi = mapRotationMatrixToRotationVector(C) 7 | % 8 | % Author(s): Dario Bellicoso 9 | 10 | phi = unskew(logm(C)); 11 | 12 | end -------------------------------------------------------------------------------- /matlab/core/mapRotationQuaternionToRotationVector.m: -------------------------------------------------------------------------------- 1 | function phi = mapRotationQuaternionToRotationVector(q) 2 | % MAPROTATIONQUATERNIONTOROTATIONVECTOR(q) extracts a rotation vector from 3 | % a unit quaternion. 4 | % 5 | % Author(s): Dario Bellicoso 6 | 7 | th = q(1); 8 | n = q(2:4); 9 | 10 | if (norm(n) < eps) 11 | if (th>0) 12 | phi = 2*n; 13 | else 14 | phi = -2*n; 15 | end 16 | else 17 | phi = 2*atan2(norm(n), th)*n/norm(n); 18 | end 19 | 20 | end -------------------------------------------------------------------------------- /matlab/core/mapRotationVectorToRotationMatrix.m: -------------------------------------------------------------------------------- 1 | function C = mapRotationVectorToRotationMatrix(phi) 2 | % MAPROTATIONVECTORTOROTATIONMATRIX maps a rotation vector phi in so(3) to 3 | % a rotation matrix in SO(3) 4 | % 5 | % syntax: C = mapRotationVectorToRotationMatrix(phi) 6 | % 7 | % Author(s): Dario Bellicoso 8 | 9 | 10 | % phi is a rotation vector which describes the rotation of a frame w.r.t. 11 | % another one. The rotation matrix in SO(3) can be computed by using the 12 | % exponential map from so(3) to SO(3) 13 | th = norm(phi); 14 | 15 | if (abs(th) < eps) 16 | C = eye(3) + skew(phi); 17 | else 18 | C = eye(3) + sin(th)/th*skew(phi) + (1-cos(th))/(th^2)*skew(phi)^2; 19 | end 20 | 21 | 22 | end -------------------------------------------------------------------------------- /matlab/core/rotateVectorUsingQuaternion.m: -------------------------------------------------------------------------------- 1 | function A_r = rotateVectorUsingQuaternion(q_AB, B_r) 2 | % ROTATEVECTORUSINGQUATERNION(q_AB, B_r) projects the components of B_r to 3 | % those of A_r. 4 | % 5 | % Author(s): Dario Bellicoso 6 | 7 | q0 = q_AB(1); 8 | qv = q_AB(2:4); 9 | qv = qv(:); 10 | sQv = skew(qv); 11 | 12 | B_r = B_r(:); 13 | 14 | A_r = (2*q0^2-1)*B_r + 2*q0*sQv*B_r + 2*qv*(qv'*B_r); 15 | 16 | end -------------------------------------------------------------------------------- /matlab/core/skew.m: -------------------------------------------------------------------------------- 1 | function S = skew(v) 2 | % SKEW(v) outputs the skew matrix of a 3d vector v. 3 | % 4 | % Author(s): Dario Bellicoso 5 | 6 | S = [ 0 -v(3) v(2); 7 | v(3) 0 -v(1); 8 | -v(2) v(1) 0 ]; 9 | 10 | end -------------------------------------------------------------------------------- /matlab/core/unskew.m: -------------------------------------------------------------------------------- 1 | function v = unskew(S) 2 | % UNSKEW(S) extracts a vector v in R^3 from a skew matrix S(v). 3 | % 4 | % Author(s): Dario Bellicoso 5 | 6 | v = [S(3,2); S(1,3); S(2,1)]; 7 | 8 | end -------------------------------------------------------------------------------- /matlab/finite_differences/linearizationExample.m: -------------------------------------------------------------------------------- 1 | % LINEARIZATIONEXAMPLE demonstrates the use of the numerical_jacobian and 2 | % the visualizeLinearization scripts. 3 | % 4 | % Author(s): Dario Bellicoso 5 | 6 | 7 | % Define a nonlinear (possibly time variant) function handle for the 1D and 8 | % the 2D case 9 | fcn_1d = @(t,x)(x.^(2)); 10 | fcn_2d = @(t,x)(x(1).^2 + x(2).^2); 11 | 12 | % Define the point around which to linearize 13 | t0 = 0; 14 | x0_1d = 2*rand - 1; 15 | x0_2d = [2*rand - 1; 2*rand - 1]; 16 | 17 | 18 | % Compute the numerical jacobian using the built in method 19 | thr_1d = 1e-6*ones(length(x0_1d),1); 20 | thr_2d = 1e-6*ones(length(x0_2d),1); 21 | dfdx_1d_matlab = numjac(fcn_1d, t0, x0_1d, fcn_1d(t0, x0_1d), thr_1d, [], 0); 22 | dfdx_2d_matlab = numjac(fcn_2d, t0, x0_2d, fcn_2d(t0, x0_2d), thr_2d, [], 0); 23 | 24 | % Compute the numerical jacobian using a custom method 25 | dfdx_1d_custom = numerical_jacobian(fcn_1d, t0, x0_1d); 26 | dfdx_2d_custom = numerical_jacobian(fcn_2d, t0, x0_2d); 27 | 28 | % Check validity of custom jacobian computation 29 | disp('Difference between 1d jacobians: '); 30 | disp(norm(dfdx_1d_matlab-dfdx_1d_custom)); 31 | 32 | disp('Difference between 2d jacobians: '); 33 | disp(norm(dfdx_2d_matlab-dfdx_2d_custom)); 34 | 35 | % Plot the nonlinear function and its linearization 36 | visualizeLinearization(fcn_1d, t0, x0_1d); 37 | visualizeLinearization(fcn_2d, t0, x0_2d); 38 | -------------------------------------------------------------------------------- /matlab/finite_differences/numerical_jacobian.m: -------------------------------------------------------------------------------- 1 | function dfdx = numerical_jacobian(fcn, t0, x0, delta) 2 | %NUMERICAL_JACOBIAN(fcn, t0, x0, delta) Jacobian of a nonlinear function 3 | % dfdx = NUMERICAL_JACOBIAN(fcn, t0, x0, delta) returns the numerical 4 | % approximation of the jacobian of a function fcn(t,x) around the tuple 5 | % (t0, x0). The optional parameter delta is the variation size. 6 | % 7 | % Author(s): Dario Bellicoso 8 | 9 | %% Setup 10 | n = length(x0); 11 | m = length(fcn(t0,x0)); 12 | dfdx = zeros(m,n); 13 | 14 | if nargin < 4 15 | delta = 1e-5; 16 | end 17 | 18 | %% Numerical approximation 19 | 20 | % Evaluate the columns of the jacobian of fcn(t,x) 21 | dx = zeros(n,1); 22 | for k=1:n 23 | % Perturb x(k) by delta 24 | dx(k) = delta; 25 | 26 | % Evaluate the k-th column of the jacobian through finite differences 27 | dfdx(:,k) = (fcn(t0, x0+dx) - fcn(t0, x0-dx))/(2*delta); 28 | 29 | % Reset perturbation 30 | dx(k) = 0; 31 | end 32 | 33 | end -------------------------------------------------------------------------------- /matlab/utils/dMATdt.m: -------------------------------------------------------------------------------- 1 | % function dA = dMATdt (A,q,dq) 2 | % 3 | % -> total time derivation of a matrix or a vector A(q). 4 | % dA(q)/dt = dA/dq*dq, 5 | % where dq is vector q derived with respect to time t. 6 | % 7 | % 8 | % proNEu: tool for symbolic EoM derivation 9 | % Copyright (C) 2011 Marco Hutter, Christian Gehring 10 | % 11 | % This program is free software: you can redistribute it and/or modify 12 | % it under the terms of the GNU General Public License as published by 13 | % the Free Software Foundation, either version 3 of the License, or 14 | % any later version. 15 | % 16 | % This program is distributed in the hope that it will be useful, 17 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | % GNU General Public License for more details. 20 | % 21 | % You should have received a copy of the GNU General Public License 22 | % along with this program. If not, see . 23 | function [ dA ] = dMATdt( A, q, dq ) 24 | 25 | dA = sym(zeros(size(A))); 26 | for i=1:size(A,1) 27 | for j=1:size(A,2) 28 | dA(i,j) = jacobian(A(i,j),q)*dq; 29 | end 30 | end 31 | end 32 | 33 | -------------------------------------------------------------------------------- /matlab/utils/fulldMATdt.m: -------------------------------------------------------------------------------- 1 | % function [ ddA ] = d2MATdt2( A, n, q, dq, ddq) 2 | % 3 | % -> nth time derivation of a matrix or a vector A(q). 4 | % dnA(q)/dtn 5 | % 6 | % Note that this function was only proved up to second derivative. 7 | % 8 | % Created by Christian Gehring on 11.10.2012 9 | % from Matlab2010a 10 | % -> cgehring@anybotics.com 11 | 12 | function [ ddA ] = fulldMATdt( A, n, q, dq, ddq) 13 | if n > 2 14 | error('This function was not checked for higher derivatives!') 15 | end 16 | ddA = fulldiff2(A,num2cell(q),n,num2cell(dq),num2cell(ddq)); 17 | end 18 | -------------------------------------------------------------------------------- /matlab/utils/rotations.m: -------------------------------------------------------------------------------- 1 | %% Kindr 1.0.0 2 | % Author(s): Christian Gehring 3 | 4 | clear all, clc 5 | 6 | %% Variables 7 | 8 | syms x y z real; 9 | syms dx dy dz real; 10 | syms w1 w2 w3 real; 11 | 12 | % angular velociy 13 | w = [w1, w2, w3]'; 14 | 15 | %% Elementary Rotations 16 | 17 | C_x = [1 0 0; 18 | 0 cos(x) -sin(x); 19 | 0 sin(x) cos(x)]; 20 | 21 | C_y = [cos(y) 0 sin(y); 22 | 0 1 0; 23 | -sin(y) 0 cos(y)]; 24 | 25 | C_z = [cos(z) -sin(z) 0; 26 | sin(z) cos(z) 0; 27 | 0 0 1]; 28 | 29 | %% Euler Angles ZYX 30 | 31 | zyx = [z, y, x]'; 32 | dzyx = [dz, dy, dx]'; 33 | 34 | % rotation matrix 35 | C_IB_zyx = simplify(C_z*C_y*C_x) 36 | 37 | % time derivative of rotation matrix 38 | dC_IB_zyx = dMATdt( C_IB_zyx, zyx, dzyx ) 39 | 40 | % local angular velocity 41 | B_w_IB = simplify(unskew(C_IB_zyx'*dC_IB_zyx)) 42 | 43 | % It must hold 44 | % w = B_w_IB or f := w - B_w_IB = 0 45 | f = w-B_w_IB; 46 | 47 | % B_w_IB = E_zyx*dzyx; 48 | res1 = solve(f, w1, w2, w3); 49 | E_zyx = [jacobian(simplify(res1.w1), dzyx) 50 | jacobian(simplify(res1.w2), dzyx) 51 | jacobian(simplify(res1.w3), dzyx)] 52 | ccode(E_zyx,'file', 'tmp/E_zyx_local.hpp') 53 | 54 | % time derivative of E_zyx 55 | dE_zyx = dMATdt(E_zyx, zyx, dzyx) 56 | ccode(dE_zyx,'file', 'tmp/dE_zyx_local.hpp') 57 | 58 | % dzyx = E_zyx_inv*B_w_IB; 59 | res2 = solve(f, dx, dy, dz); 60 | E_zyx_inv = [jacobian(simplify(res2.dz), w) 61 | jacobian(simplify(res2.dy), w) 62 | jacobian(simplify(res2.dx), w)] 63 | ccode(E_zyx_inv,'file', 'tmp/E_zyx_local_inv.hpp') 64 | 65 | % time derivative of E_zyx_inv 66 | dE_zyx_inv = dMATdt(E_zyx_inv, zyx, dzyx) 67 | ccode(dE_zyx_inv,'file', 'tmp/dE_zyx_local_inv.hpp') 68 | 69 | % global angular velocity 70 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 71 | 72 | I_w_IB = simplify(unskew(dC_IB_zyx*C_IB_zyx')) 73 | f_global = w-I_w_IB; 74 | 75 | % B_w_IB = E_zyx*dzyx; 76 | res1_global = solve(f_global, w1, w2, w3); 77 | E_zyx_global = [jacobian(simplify(res1_global.w1), dzyx) 78 | jacobian(simplify(res1_global.w2), dzyx) 79 | jacobian(simplify(res1_global.w3), dzyx)] 80 | ccode(E_zyx_global,'file', 'tmp/E_zyx_global.hpp') 81 | 82 | % time derivative of E_zyx 83 | dE_zyx_global = dMATdt(E_zyx_global, zyx, dzyx) 84 | ccode(dE_zyx_global,'file', 'tmp/dE_zyx_global.hpp') 85 | 86 | % dzyx = E_zyx_inv*B_w_IB; 87 | res2_global = solve(f_global, dz, dy, dx); 88 | E_zyx_global_inv = [jacobian(simplify(res2_global.dz), w) 89 | jacobian(simplify(res2_global.dy), w) 90 | jacobian(simplify(res2_global.dx), w)] 91 | ccode(E_zyx_global_inv,'file', 'tmp/E_zyx_global_inv.hpp') 92 | 93 | % time derivative of E_zyx_inv 94 | dE_zyx_global_inv = dMATdt(E_zyx_global_inv, zyx, dzyx) 95 | ccode(dE_zyx_global_inv,'file', 'tmp/dE_zyx_global_inv.hpp') 96 | 97 | 98 | %% Euler Angles XYZ 99 | 100 | xyz = [x, y, z]'; 101 | dxyz = [dx, dy, dz]'; 102 | 103 | % rotation matrix 104 | C_IB_xyz = simplify(C_x*C_y*C_z) 105 | 106 | % time derivative of rotation matrix 107 | dC_IB_xyz = dMATdt( C_IB_xyz, xyz, dxyz ) 108 | 109 | % local angular velocity 110 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 111 | B_w_IB = simplify(unskew(C_IB_xyz'*dC_IB_xyz)) 112 | 113 | % It must hold 114 | % w = B_w_IB or f := w - B_w_IB = 0 115 | f_local = w-B_w_IB; 116 | 117 | % B_w_IB = E_zyx*dzyx; 118 | res1_local = solve(f_local, w1, w2, w3); 119 | E_xyz_local = [jacobian(simplify(res1_local.w1), dxyz) 120 | jacobian(simplify(res1_local.w2), dxyz) 121 | jacobian(simplify(res1_local.w3), dxyz)] 122 | ccode(E_xyz_local,'file', 'tmp/E_xyz_local.hpp') 123 | 124 | % time derivative of E_zyx 125 | dE_xyz_local = dMATdt(E_xyz_local, xyz, dxyz) 126 | ccode(dE_xyz_local,'file', 'tmp/dE_xyz_local.hpp') 127 | 128 | % dzyx = E_zyx_inv*B_w_IB; 129 | res2_local = solve(f_local, dx, dy, dz); 130 | E_xyz_local_inv = [jacobian(simplify(res2_local.dx), w) 131 | jacobian(simplify(res2_local.dy), w) 132 | jacobian(simplify(res2_local.dz), w)] 133 | ccode(E_xyz_local_inv,'file', 'tmp/E_xyz_local_inv.hpp') 134 | 135 | % time derivative of E_zyx_inv 136 | dE_xyz_local_inv = dMATdt(E_xyz_local_inv, xyz, dxyz) 137 | ccode(dE_xyz_local_inv,'file', 'tmp/dE_xyz_local_inv.hpp') 138 | 139 | % global angular velocity 140 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 141 | 142 | I_w_IB = simplify(unskew(dC_IB_xyz*C_IB_xyz')) 143 | f_global = w-I_w_IB; 144 | 145 | % B_w_IB = E_zyx*dzyx; 146 | res1_global = solve(f_global, w1, w2, w3); 147 | E_xyz_global = [jacobian(simplify(res1_global.w1), dxyz) 148 | jacobian(simplify(res1_global.w2), dxyz) 149 | jacobian(simplify(res1_global.w3), dxyz)] 150 | ccode(E_xyz_global,'file', 'tmp/E_xyz_global.hpp') 151 | 152 | % time derivative of E_zyx 153 | dE_xyz_global = dMATdt(E_xyz_global, xyz, dxyz) 154 | ccode(dE_xyz_global,'file', 'tmp/dE_xyz_global.hpp') 155 | 156 | % dzyx = E_zyx_inv*B_w_IB; 157 | res2_global = solve(f_global, dx, dy, dz); 158 | E_xyz_global_inv = [jacobian(simplify(res2_global.dx), w) 159 | jacobian(simplify(res2_global.dy), w) 160 | jacobian(simplify(res2_global.dz), w)] 161 | ccode(E_xyz_global_inv,'file', 'tmp/E_xyz_global_inv.hpp') 162 | 163 | % time derivative of E_zyx_inv 164 | dE_xyz_global_inv = dMATdt(E_xyz_global_inv, xyz, dxyz) 165 | ccode(dE_xyz_global_inv,'file', 'tmp/dE_xyz_global_inv.hpp') -------------------------------------------------------------------------------- /matlab/visualization/addLabelToVector.m: -------------------------------------------------------------------------------- 1 | function [axisHandle, labelHandle] = addLabelToVector(axisHandle, vec, str) 2 | % ADDLABELTOVECTOR(axisHandle, vec, str) adds a text label to a vector in a 3 | % figure. 4 | % 5 | % Author(s): Dario Bellicoso 6 | 7 | % Make h the current figure 8 | axes(axisHandle); 9 | 10 | % Generate a random displacement vector. This helps to avoid overlapping of 11 | % labels 12 | deltaDisplacement = 2*rand(3,1)-1; 13 | delta = 0.1*deltaDisplacement/norm(deltaDisplacement); 14 | 15 | % Add the text 16 | labelHandle = text(vec(1)+delta(1), vec(2)+delta(2), vec(3)+delta(3), ['$$' str '$$'], 'Interpreter', 'Latex'); 17 | 18 | end -------------------------------------------------------------------------------- /matlab/visualization/animateAngularVelocityUsingEulerZYX.m: -------------------------------------------------------------------------------- 1 | function animateAngularVelocityUsingEulerZYX(I_w_IB, rate, updateFrequency) 2 | % ANIMATEANGULARVELOCITY(I_w_IB, rate, updateFrequency) generates an 3 | % animation showing the rotation of a coordinate system w.r.t. a fixed one. 4 | % 5 | % Author(s): Dario Bellicoso 6 | 7 | % Provide a default scaling for the angular velocity 8 | if nargin < 2 9 | rate = 1; 10 | end 11 | 12 | % Provide a default redraw frequency 13 | if nargin < 3 14 | updateFrequency = 25; % Hz 15 | end 16 | 17 | % Setup figure 18 | limit = 1.5; 19 | limitsX = [-limit limit]; 20 | limitsY = [-limit limit]; 21 | limitsZ = [-limit limit]; 22 | figure(); 23 | axHandle = axes(); 24 | view(3); 25 | hold on; grid on; axis equal; 26 | 27 | % Get scalar angular velocity and rotation direction 28 | I_w_IB = I_w_IB*rate; 29 | w = norm(I_w_IB); 30 | w_vec = I_w_IB/w; 31 | 32 | % Animate for t_f seconds 33 | t_f = 30.0; 34 | 35 | % Define how many 'old' coordinate frame to show. This emulates fading out 36 | % of the coordinate frame animation. Use 1 to just show the latest update 37 | traceMemory = 1; 38 | 39 | % Discrete steps to jump when visualizing an older coordinate frame 40 | skipSteps = 5; 41 | 42 | % Visualize the coordinate frame unit vectors and get handles to them 43 | [axHandle, inertialFrameHandle] = visualizeCoordinateSystem(eye(3), axHandle, 'I'); 44 | 45 | % Draw an ellipsoid as a rigid body representation 46 | bodyScale = 0.5; 47 | [x, y, z] = ellipsoid(0, 0, 0, bodyScale*1.0, bodyScale*0.75, bodyScale*0.5, 50); 48 | bodyHandle = surf(x, y, z, 2*z); 49 | set(bodyHandle, 'edgecolor','none'); 50 | 51 | % Initialize containers 52 | transforms = cell(traceMemory,1); 53 | handles = cell(traceMemory,1); 54 | colorNorms = linspace(0, 1, traceMemory); 55 | colorNorms = fliplr(colorNorms); 56 | 57 | C_IB = cell(traceMemory,1); 58 | for k=1:traceMemory 59 | C_IB{k} = eye(3); 60 | end 61 | 62 | for k=1:traceMemory 63 | % Use axis labels only for the first rotation set 64 | if k==1 65 | [axHandle, frameHandle] = visualizeCoordinateSystem(eye(3), axHandle, 'B'); 66 | else 67 | [axHandle, frameHandle] = visualizeCoordinateSystem(eye(3), axHandle); 68 | end 69 | 70 | % Decrease the color norm for older rotation sets 71 | for kHandle=1:3 72 | handleColor = get(frameHandle(kHandle), 'Color'); 73 | set(frameHandle(kHandle), 'Color', handleColor/norm(handleColor)*colorNorms(k)); 74 | 75 | scale = 1 + (k-1)/10; 76 | 77 | udata = get(frameHandle(kHandle),'UData'); 78 | set(frameHandle(kHandle),'UData', udata/scale); 79 | 80 | vdata = get(frameHandle(kHandle),'VData'); 81 | set(frameHandle(kHandle),'VData', vdata/scale); 82 | 83 | wdata = get(frameHandle(kHandle),'WData'); 84 | set(frameHandle(kHandle),'WData', wdata/scale); 85 | end 86 | 87 | % Associate a transformation to each rotation set 88 | handles{k} = frameHandle; 89 | transforms{k} = hgtransform('Parent', axHandle); 90 | set(handles{k}, 'Parent', transforms{k}); 91 | 92 | end 93 | 94 | % Display the angular velocity pseudovector and annotate it 95 | quiver3(0, 0, 0, w_vec(1), w_vec(2), w_vec(3), 'k', 'LineWidth', 2); 96 | addLabelToVector(axHandle, w_vec, '{}_I \omega_{IB}'); 97 | 98 | % Setup the transforms for the coordinate systems 99 | tInterial = hgtransform('Parent', axHandle); 100 | set(inertialFrameHandle,'Parent',tInterial); 101 | tBody = hgtransform('Parent', axHandle); 102 | set(bodyHandle,'Parent',tBody); 103 | 104 | % Set the axis limits 105 | xlim(axHandle, limitsX); 106 | ylim(axHandle, limitsY); 107 | zlim(axHandle, limitsZ); 108 | 109 | % Initial orientation using ZYX Euler angles. Angles are stored in z-y-x 110 | % order 111 | angles = [0;0;0]; 112 | 113 | for k=1:(t_f*updateFrequency) 114 | % Start a timer 115 | tic; 116 | 117 | dt = 1/updateFrequency; 118 | 119 | % Construct the current rotation vector 120 | for g=1:traceMemory 121 | tk = (k - (skipSteps*g-skipSteps)) / updateFrequency; 122 | 123 | % Map the rotation vector to the rotation matrix in SO(3) 124 | E = getMapEulAngZYXDiffToAngVelInWorldFrame(angles); 125 | if (rank(E) < 3 || abs(det(E)) < 0.1) 126 | disp('Mapping is singular!'); 127 | disp(['Current angles: ' num2str(angles(1)) ' ' num2str(angles(2)) ' ' num2str(angles(3))]); 128 | %break; 129 | end 130 | anglesDot = E\I_w_IB; 131 | %angles = angles + dt*anglesDot; 132 | angles = tk*anglesDot; 133 | C_IB{g,1} = mapEulerAnglesZYXToRotationMatrix(angles); 134 | 135 | 136 | % todo: rotation matrix check sometimes fails 137 | %[isRot, errMsg] = isRotationMatrix(C_IB{g,1}); 138 | %errMsg = [errMsg ' Time step: ' num2str(k)]; 139 | %assert(isRot, errMsg); 140 | 141 | % Visualize the rotation vector 142 | try 143 | set(transforms{g}, 'Matrix', [C_IB{g,1} zeros(3,1); zeros(1,3) 1]); 144 | catch 145 | break; 146 | end 147 | 148 | end 149 | 150 | % Rotate the rigid body 151 | try 152 | set(tBody, 'Matrix', [C_IB{1,1} zeros(3,1); zeros(1,3) 1]); 153 | catch 154 | break; 155 | end 156 | 157 | try 158 | drawnow; 159 | catch 160 | break; 161 | end 162 | 163 | % Try to synchronize the update loop 164 | waitTime = dt - toc; 165 | if (waitTime > 0) 166 | pause(dt-toc); 167 | end 168 | 169 | end 170 | 171 | end 172 | -------------------------------------------------------------------------------- /matlab/visualization/animateAngularVelocityUsingExpMap.m: -------------------------------------------------------------------------------- 1 | function animateAngularVelocityUsingExpMap(I_w_IB, rate, updateFrequency) 2 | % ANIMATEANGULARVELOCITYUSINGEXPMAP(I_w_IB, rate, updateFrequency) 3 | % generates an animation showing the rotation of a coordinate system w.r.t. 4 | % a fixed one. 5 | % 6 | % Author(s): Dario Bellicoso 7 | 8 | % Provide a default scaling for the angular velocity 9 | if nargin < 2 10 | rate = 1; 11 | end 12 | 13 | % Provide a default redraw frequency 14 | if nargin < 3 15 | updateFrequency = 25; % Hz 16 | end 17 | 18 | % Setup figure 19 | limit = 1.5; 20 | limitsX = [-limit limit]; 21 | limitsY = [-limit limit]; 22 | limitsZ = [-limit limit]; 23 | figure(); 24 | axHandle = axes(); 25 | view(3); 26 | hold on; grid on; axis equal; 27 | 28 | % Get scalar angular velocity and rotation direction 29 | I_w_IB = I_w_IB*rate; 30 | w = norm(I_w_IB); 31 | w_vec = I_w_IB/w; 32 | 33 | % Animate for t_f seconds 34 | t_f = 30.0; 35 | 36 | % Define how many 'old' coordinate frame to show. This emulates fading out 37 | % of the coordinate frame animation. Use 1 to just show the latest update 38 | traceMemory = 1; 39 | 40 | % Discrete steps to jump when visualizing an older coordinate frame 41 | skipSteps = 5; 42 | 43 | % Visualize the coordinate frame unit vectors and get handles to them 44 | [axHandle, inertialFrameHandle] = visualizeCoordinateSystem(eye(3), axHandle, 'I'); 45 | 46 | % Draw an ellipsoid as a rigid body representation 47 | bodyScale = 0.5; 48 | [x, y, z] = ellipsoid(0, 0, 0, bodyScale*1.0, bodyScale*0.75, bodyScale*0.5, 50); 49 | bodyHandle = surf(x, y, z, 2*z); 50 | set(bodyHandle, 'edgecolor','none'); 51 | 52 | % Initialize containers 53 | transforms = cell(traceMemory,1); 54 | handles = cell(traceMemory,1); 55 | colorNorms = linspace(0, 1, traceMemory); 56 | colorNorms = fliplr(colorNorms); 57 | 58 | C_IB = cell(traceMemory,1); 59 | for k=1:traceMemory 60 | C_IB{k} = eye(3); 61 | end 62 | 63 | for k=1:traceMemory 64 | % Use axis labels only for the first rotation set 65 | if k==1 66 | [axHandle, frameHandle] = visualizeCoordinateSystem(eye(3), axHandle, 'B'); 67 | else 68 | [axHandle, frameHandle] = visualizeCoordinateSystem(eye(3), axHandle); 69 | end 70 | 71 | % Decrease the color norm for older rotation sets 72 | for kHandle=1:3 73 | handleColor = get(frameHandle(kHandle), 'Color'); 74 | set(frameHandle(kHandle), 'Color', handleColor/norm(handleColor)*colorNorms(k)); 75 | 76 | scale = 1 + (k-1)/10; 77 | 78 | udata = get(frameHandle(kHandle),'UData'); 79 | set(frameHandle(kHandle),'UData', udata/scale); 80 | 81 | vdata = get(frameHandle(kHandle),'VData'); 82 | set(frameHandle(kHandle),'VData', vdata/scale); 83 | 84 | wdata = get(frameHandle(kHandle),'WData'); 85 | set(frameHandle(kHandle),'WData', wdata/scale); 86 | end 87 | 88 | % Associate a transformation to each rotation set 89 | handles{k} = frameHandle; 90 | transforms{k} = hgtransform('Parent', axHandle); 91 | set(handles{k}, 'Parent', transforms{k}); 92 | 93 | end 94 | 95 | % Display the angular velocity pseudovector and annotate it 96 | quiver3(0, 0, 0, w_vec(1), w_vec(2), w_vec(3), 'k', 'LineWidth', 2); 97 | addLabelToVector(axHandle, w_vec, '{}_I \omega_{IB}'); 98 | 99 | % Setup the transforms for the coordinate systems 100 | tInterial = hgtransform('Parent', axHandle); 101 | set(inertialFrameHandle,'Parent',tInterial); 102 | tBody = hgtransform('Parent', axHandle); 103 | set(bodyHandle,'Parent',tBody); 104 | 105 | % Set the axis limits 106 | xlim(axHandle, limitsX); 107 | ylim(axHandle, limitsY); 108 | zlim(axHandle, limitsZ); 109 | 110 | for k=1:(t_f*updateFrequency) 111 | % Start a timer 112 | tic; 113 | 114 | % Construct the current rotation vector 115 | for g=1:traceMemory 116 | tk = (k - (skipSteps*g-skipSteps)) / updateFrequency; 117 | 118 | % Map the rotation vector to the rotation matrix in SO(3) 119 | phi_IB = tk*I_w_IB; 120 | C_IB{g,1} = expm(skew(phi_IB)); 121 | 122 | % todo: rotation matrix check sometimes fails 123 | %[isRot, errMsg] = isRotationMatrix(C_IB{g,1}); 124 | %errMsg = [errMsg ' Time step: ' num2str(k)]; 125 | %assert(isRot, errMsg); 126 | 127 | % Visualize the rotation vector 128 | try 129 | set(transforms{g}, 'Matrix', [C_IB{g,1} zeros(3,1); zeros(1,3) 1]); 130 | catch 131 | break; 132 | end 133 | 134 | end 135 | 136 | % Rotate the rigid body 137 | try 138 | set(tBody, 'Matrix', [C_IB{1,1} zeros(3,1); zeros(1,3) 1]); 139 | catch 140 | break; 141 | end 142 | 143 | try 144 | drawnow; 145 | catch 146 | break; 147 | end 148 | 149 | % Try to synchronize the update loop 150 | dt = 1/updateFrequency; 151 | waitTime = dt - toc; 152 | if (waitTime > 0) 153 | pause(dt-toc); 154 | end 155 | 156 | end 157 | 158 | end 159 | -------------------------------------------------------------------------------- /matlab/visualization/animateLinearVelocity.m: -------------------------------------------------------------------------------- 1 | function animateLinearVelocity() 2 | % ANIMATELINEARVELOCITY() 3 | % generates an animation showing the rotation of a coordinate system w.r.t. 4 | % a fixed one. 5 | % 6 | % Author(s): Dario Bellicoso 7 | 8 | % Provide a default scaling for the angular velocity 9 | if nargin < 2 10 | rate = 1; 11 | end 12 | 13 | % Provide a default redraw frequency 14 | if nargin < 3 15 | updateFrequency = 50; % Hz 16 | end 17 | 18 | % Setup figure 19 | limit = 2.0; 20 | limitsX = [-limit limit]; 21 | limitsY = [-limit limit]; 22 | limitsZ = [-limit limit]; 23 | figure(); 24 | axHandle = axes(); 25 | %view(3); 26 | hold on; grid on; axis equal; 27 | 28 | % Set the axis limits 29 | xlim(axHandle, limitsX); 30 | ylim(axHandle, limitsY); 31 | zlim(axHandle, limitsZ); 32 | 33 | % Animate for t_f seconds 34 | t_f = 90.0; 35 | 36 | % Visualize the coordinate frame unit vectors and get handles to them 37 | [axHandle, iFrameHandle] = visualizeCoordinateSystem(eye(3), axHandle, 'I'); 38 | [axHandle, bFrameHandle] = visualizeCoordinateSystem(eye(3), axHandle, 'B'); 39 | 40 | % Draw an ellipsoid as a rigid body representation 41 | bodyScale = 0.05; 42 | bodyDim = [1.0; 1.0; 1.0]; 43 | [x, y, z] = ellipsoid(0, 0, 0, bodyScale*bodyDim(1), bodyScale*bodyDim(2), bodyScale*bodyDim(3), 50); 44 | bodyHandle = surf(x, y, z, 2*z); 45 | set(bodyHandle, 'edgecolor','none'); 46 | 47 | % Setup the transforms for the coordinate systems 48 | tIFrame = hgtransform('Parent', axHandle); 49 | tBFrame = hgtransform('Parent', axHandle); 50 | tBody = hgtransform('Parent', axHandle); 51 | set(iFrameHandle,'Parent',tIFrame); 52 | set(bFrameHandle,'Parent',tBFrame); 53 | set(bodyHandle,'Parent',tBody); 54 | 55 | % Initial pose of frame B 56 | I_r_IB = rand(3,1); 57 | ph_IB = [0; 0; rand(1,1)]; 58 | C_IB = mapRotationVectorToRotationMatrix(ph_IB); 59 | T_IB = [C_IB I_r_IB; zeros(1,3) 1]; 60 | set(tBFrame, 'Matrix', T_IB); 61 | 62 | % Body motion settings 63 | sineFreq = 0.1; % sine frequency [Hz] 64 | sineA = 0.4; % sine amplitude 65 | B_r_BP = sineA*[cos(0); sin(0); 0]; 66 | I_r_IP = I_r_IB + C_IB*(B_r_BP); 67 | B_r_IP = C_IB'*I_r_IP; 68 | 69 | bodyVelocityInInertialFrameHandle = quiver3(0, 0, 0, 0, 0, 0, 'r', 'LineWidth', 1); 70 | bodyVelocityInMovingFrameHandle = quiver3(0, 0, 0, 0, 0, 0, 'b', 'LineWidth', 1); 71 | bFrameVelocityHandle = quiver3(0, 0, 0, 0, 0, 0, 'm', 'LineWidth', 1); 72 | angVel = 0.5; 73 | 74 | % Create transforms for trace. 75 | traceLength = 70; 76 | pointTransformsBodyFrame = cell(traceLength,1); 77 | pointTransformsInertialFrame = cell(traceLength,1); 78 | pointTransformsMovingFrame = cell(traceLength,1); 79 | pointHistoryBodyFrame = cell(traceLength,1); 80 | pointHistoryInertialFrame = cell(traceLength,1); 81 | pointHistoryMovingFrame = cell(traceLength,1); 82 | for k=1:traceLength 83 | pointHandleBodyFrame = plot3(0, 0, 0, 'b.'); 84 | pointHandleInertialFrame = plot3(0, 0, 0, 'r.'); 85 | pointHandleMovingFrame = plot3(0, 0, 0, 'k.'); 86 | 87 | pointTransformsBodyFrame{k} = hgtransform('Parent', axHandle); 88 | pointTransformsInertialFrame{k} = hgtransform('Parent', axHandle); 89 | pointTransformsMovingFrame{k} = hgtransform('Parent', axHandle); 90 | 91 | set(pointHandleBodyFrame,'Parent',pointTransformsBodyFrame{k}); 92 | set(pointHandleInertialFrame,'Parent',pointTransformsInertialFrame{k}); 93 | set(pointHandleMovingFrame,'Parent',pointTransformsMovingFrame{k}); 94 | 95 | pointHistoryBodyFrame{k} = [0;0;0]; 96 | pointHistoryInertialFrame{k} = [0;0;0]; 97 | pointHistoryMovingFrame{k} = [0;0;0]; 98 | end 99 | 100 | skipUpdates = 4; 101 | counter = 1; 102 | 103 | dt = 1/updateFrequency; 104 | currentInterTime = dt; 105 | for k=1:(t_f*updateFrequency) 106 | tic; 107 | if (k~=1) 108 | %currentInterTime = toc(loopStartTime); 109 | end 110 | 111 | % Start a timer. 112 | loopStartTime = tic; 113 | 114 | % Set angular motion of frame B. 115 | sineParam = 2*pi*sineFreq*k*currentInterTime; 116 | I_w_IB = [0; 0; angVel]; 117 | ph_IB = ph_IB + I_w_IB*currentInterTime; 118 | C_IB = mapRotationVectorToRotationMatrix(ph_IB); 119 | %C_IB = expm(currentInterTime*skew(I_w_IB)); 120 | 121 | % Set linear motion of frame B. 122 | I_v_IB = 2*pi*sineFreq*sineA*[-sin(sineParam); cos(sineParam); 0]; 123 | I_r_IB = I_r_IB + I_v_IB*currentInterTime; 124 | 125 | % Set motion of the body. 126 | B_dr_BP = 2*pi*sineFreq*sineA*[-sin(sineParam); cos(sineParam); 0]; 127 | B_r_BP = B_r_BP + B_dr_BP*currentInterTime; 128 | I_r_BP = C_IB*B_r_BP; 129 | 130 | % Integrate motion. 131 | I_v_IP = I_v_IB + C_IB*B_dr_BP + cross(I_w_IB, I_r_BP); 132 | %I_r_IP = I_r_IP + I_v_IP/updateFrequency; 133 | 134 | % Test: get motion from a moving frame. 135 | B_w_IB = C_IB'*I_w_IB; 136 | B_w_BI = -B_w_IB; 137 | 138 | 139 | 140 | B_v_IP = C_IB'*I_v_IP + cross(B_w_BI, B_r_IP); 141 | %B_v_IP = C_IB'*I_v_IP + cross(B_w_IB, B_r_IP); 142 | 143 | 144 | 145 | 146 | 147 | B_r_IP = B_r_IP + B_v_IP*currentInterTime; 148 | I_r_IP = C_IB*B_r_IP; 149 | 150 | I_v_BP = C_IB*B_dr_BP; 151 | 152 | % Update transforms. 153 | try 154 | %plot3(I_r_IP(1), I_r_IP(2), I_r_IP(3), 'b.'); 155 | set(bodyVelocityInInertialFrameHandle, 'XData', I_r_IP(1)); 156 | set(bodyVelocityInInertialFrameHandle, 'YData', I_r_IP(2)); 157 | set(bodyVelocityInInertialFrameHandle, 'ZData', I_r_IP(3)); 158 | set(bodyVelocityInInertialFrameHandle, 'UData', I_v_IP(1)); 159 | set(bodyVelocityInInertialFrameHandle, 'VData', I_v_IP(2)); 160 | set(bodyVelocityInInertialFrameHandle, 'WData', I_v_IP(3)); 161 | 162 | set(bodyVelocityInMovingFrameHandle, 'XData', I_r_IP(1)); 163 | set(bodyVelocityInMovingFrameHandle, 'YData', I_r_IP(2)); 164 | set(bodyVelocityInMovingFrameHandle, 'ZData', I_r_IP(3)); 165 | set(bodyVelocityInMovingFrameHandle, 'UData', I_v_BP(1)); 166 | set(bodyVelocityInMovingFrameHandle, 'VData', I_v_BP(2)); 167 | set(bodyVelocityInMovingFrameHandle, 'WData', I_v_BP(3)); 168 | 169 | T_IP = [C_IB I_r_IP; zeros(1,3) 1]; 170 | set(tBody, 'Matrix', T_IP); 171 | 172 | T_IB = [C_IB I_r_IB; zeros(1,3) 1]; 173 | set(tBFrame, 'Matrix', T_IB); 174 | 175 | if (mod(k,skipUpdates) == 0) 176 | idx = mod(counter, traceLength) + 1; 177 | pointHistoryBodyFrame{idx} = B_r_BP; 178 | pointHistoryInertialFrame{idx} = I_r_IP; 179 | pointHistoryMovingFrame{idx} = I_r_IB; 180 | counter = counter + 1; 181 | end 182 | 183 | for kk=1:traceLength 184 | T_BP = [C_IB I_r_IB + C_IB*pointHistoryBodyFrame{kk}; zeros(1,3) 1]; 185 | set(pointTransformsBodyFrame{kk}, 'Matrix', T_BP); 186 | 187 | T_IP = [C_IB pointHistoryInertialFrame{kk}; zeros(1,3) 1]; 188 | set(pointTransformsInertialFrame{kk}, 'Matrix', T_IP); 189 | 190 | T_IB = [C_IB pointHistoryMovingFrame{kk}; zeros(1,3) 1]; 191 | set(pointTransformsMovingFrame{kk}, 'Matrix', T_IB); 192 | end 193 | 194 | catch 195 | disp('error while updating'); 196 | break; 197 | end 198 | 199 | try 200 | drawnow; 201 | catch 202 | break; 203 | end 204 | 205 | % Try to synchronize the update loop 206 | 207 | waitTime = dt - toc; 208 | if (waitTime > 0) 209 | pause(dt-toc); 210 | end 211 | 212 | end 213 | 214 | end 215 | -------------------------------------------------------------------------------- /matlab/visualization/visualizeCoordinateSystem.m: -------------------------------------------------------------------------------- 1 | function [h, frameHandle] = visualizeCoordinateSystem(C_AB, h, name, lineWidth) 2 | % VISUALIZECOORDINATESYSTEM visualizes a Cartesian coordinate system which 3 | % is rotated according to the rotation matrix C_AB w.r.t. a fixed frame. 4 | % The visualization will update an existing axes with handle h or create a 5 | % new one. 6 | % 7 | % Author(s): Dario Bellicoso 8 | 9 | % Check inputs 10 | [res, str] = isRotationMatrix(C_AB); 11 | if (res == 0) 12 | msg = ['C_AB is not a valid rotation matrix! Cause: ', str]; 13 | error(msg); 14 | end 15 | 16 | if nargin == 1 17 | h = gca(); 18 | useName = 0; 19 | lineWidth = 1; 20 | elseif nargin == 2 21 | axes(h); 22 | useName = 0; 23 | lineWidth = 1; 24 | elseif nargin == 3 25 | axes(h); 26 | useName = 1; 27 | name = ['_' name]; 28 | lineWidth = 1; 29 | elseif nargin == 4 30 | axes(h); 31 | useName = 1; 32 | name = ['_' name]; 33 | else 34 | h = gca(); 35 | C_AB = eye(3); 36 | name = '_I'; 37 | useName = 1; 38 | lineWidth = 1; 39 | end 40 | hold on; grid on; 41 | 42 | B_u_BX = [1 0 0]'; 43 | B_u_BY = [0 1 0]'; 44 | B_u_BZ = [0 0 1]'; 45 | 46 | A_u_BX = C_AB*B_u_BX; 47 | A_u_BY = C_AB*B_u_BY; 48 | A_u_BZ = C_AB*B_u_BZ; 49 | 50 | frameHandle(1) = quiver3(0, 0, 0, A_u_BX(1), A_u_BX(2), A_u_BX(3), 0, 'r', 'LineWidth', lineWidth); 51 | frameHandle(2) = quiver3(0, 0, 0, A_u_BY(1), A_u_BY(2), A_u_BY(3), 0, 'g', 'LineWidth', lineWidth); 52 | frameHandle(3) = quiver3(0, 0, 0, A_u_BZ(1), A_u_BZ(2), A_u_BZ(3), 0, 'b', 'LineWidth', lineWidth); 53 | 54 | if useName 55 | [h, textHandle] = addLabelToVector(h, A_u_BX, ['x' name]); 56 | frameHandle = [frameHandle textHandle]; 57 | 58 | [h, textHandle] = addLabelToVector(h, A_u_BY, ['y' name]); 59 | frameHandle = [frameHandle textHandle]; 60 | 61 | [h, textHandle] = addLabelToVector(h, A_u_BZ, ['z' name]); 62 | frameHandle = [frameHandle textHandle]; 63 | end 64 | 65 | end -------------------------------------------------------------------------------- /matlab/visualization/visualizeHomogeneousTransform.m: -------------------------------------------------------------------------------- 1 | function [h, frameHandle] = visualizeHomogeneousTransform(T_AB, h, name, lineWidth) 2 | % VISUALIZEHOMOGENEOUSTRANSFORM visualizes a Cartesian coordinate system 3 | % which is rotated according to the rotation matrix C_AB w.r.t. a fixed 4 | % frame. The visualization will update an existing axes with handle h or 5 | % create a new one. 6 | % 7 | % Author(s): Dario Bellicoso 8 | 9 | % Check inputs 10 | C_AB = T_AB(1:3, 1:3); 11 | A_r_AB = T_AB(1:3,4); 12 | 13 | [res, str] = isRotationMatrix(C_AB); 14 | if (res == 0) 15 | msg = ['C_AB is not a valid rotation matrix! Cause: ', str]; 16 | error(msg); 17 | end 18 | 19 | if nargin == 1 20 | h = gca(); 21 | useName = 0; 22 | lineWidth = 1; 23 | elseif nargin == 2 24 | axes(h); 25 | useName = 0; 26 | lineWidth = 1; 27 | elseif nargin == 3 28 | axes(h); 29 | useName = 1; 30 | name = ['_' name]; 31 | lineWidth = 1; 32 | elseif nargin == 4 33 | axes(h); 34 | useName = 1; 35 | name = ['_' name]; 36 | else 37 | h = gca(); 38 | C_AB = eye(3); 39 | name = '_I'; 40 | useName = 1; 41 | lineWidth = 1; 42 | end 43 | hold on; grid on; 44 | 45 | line_length = 0.05; 46 | B_u_BX = line_length*[1 0 0]'; 47 | B_u_BY = line_length*[0 1 0]'; 48 | B_u_BZ = line_length*[0 0 1]'; 49 | 50 | A_u_BX = C_AB*B_u_BX; 51 | A_u_BY = C_AB*B_u_BY; 52 | A_u_BZ = C_AB*B_u_BZ; 53 | 54 | frameHandle(1) = quiver3(A_r_AB(1), A_r_AB(2), A_r_AB(3), ... 55 | A_u_BX(1), A_u_BX(2), A_u_BX(3), 0, ... 56 | 'r', 'LineWidth', lineWidth); 57 | frameHandle(2) = quiver3(A_r_AB(1), A_r_AB(2), A_r_AB(3), ... 58 | A_u_BY(1), A_u_BY(2), A_u_BY(3), 0, ... 59 | 'g', 'LineWidth', lineWidth); 60 | frameHandle(3) = quiver3(A_r_AB(1), A_r_AB(2), A_r_AB(3), ... 61 | A_u_BZ(1), A_u_BZ(2), A_u_BZ(3), ... 62 | 0, 'b', 'LineWidth', lineWidth); 63 | 64 | if useName 65 | [h, textHandle] = addLabelToVector(h, A_u_BX, ['x' name]); 66 | frameHandle = [frameHandle textHandle]; 67 | 68 | [h, textHandle] = addLabelToVector(h, A_u_BY, ['y' name]); 69 | frameHandle = [frameHandle textHandle]; 70 | 71 | [h, textHandle] = addLabelToVector(h, A_u_BZ, ['z' name]); 72 | frameHandle = [frameHandle textHandle]; 73 | end 74 | 75 | end -------------------------------------------------------------------------------- /matlab/visualization/visualizeLinearization.m: -------------------------------------------------------------------------------- 1 | function visualizeLinearization(fcn, t0, x0) 2 | % VISUALIZELINEARIZATION(fcn, t0, x0) visualizes a function whos domain is 3 | % scalar or bidimensional and its linearization around the tuple (t0, x0). 4 | % The function is specified by a handle in the form f(t,x). 5 | % 6 | % Author(s): Dario Bellicoso 7 | 8 | % Compute the numerical approximation of the function around (t0,x0) 9 | dfdp = numerical_jacobian(fcn, t0, x0); 10 | 11 | % Check the domain dimension 12 | if (length(x0) == 1) 13 | % Scalar domain 14 | 15 | % Setup 16 | n = 1000; 17 | rangeLin = 0.5; 18 | range = 1; 19 | 20 | % Prepare a figure 21 | figure('name', 'Linearization of function with scalar domain'); 22 | hold on; grid on; 23 | axis equal; 24 | 25 | % Domain for the input function 26 | x = linspace(x0-range, x0+range, n); 27 | 28 | % Domain for linearized function 29 | xlin = linspace(x0-rangeLin, x0+rangeLin, n); 30 | 31 | y = fcn(t0, x); 32 | ylin = fcn(t0, x0) + dfdp*(xlin-x0); 33 | 34 | plot(x, y, 'b'); 35 | plot(xlin, ylin, 'r'); 36 | 37 | scale = -0.5; 38 | quiver(x0, fcn(t0,x0), scale*dfdp, -scale, 'LineWidth', 2); 39 | plot(x0, fcn(t0,x0), 'rx'); 40 | 41 | elseif (length(x0) == 2) 42 | % 2d domain 43 | 44 | % Setup 45 | n = 200; 46 | rangeLin = 0.5; 47 | range = 2; 48 | 49 | % Prepare a figure 50 | figure('name', 'Linearization of function with bidimensional domain'); 51 | hold on; grid on; 52 | axis equal; 53 | 54 | % Domain for the input function 55 | x = linspace(x0(1)-range, x0(1)+range, n); 56 | y = linspace(x0(2)-range, x0(2)+range, n); 57 | 58 | % Domain for linearized function 59 | xlin = linspace(x0(1)-rangeLin, x0(1)+rangeLin, n); 60 | ylin = linspace(x0(2)-rangeLin, x0(2)+rangeLin, n); 61 | 62 | [Xlin,Ylin] = meshgrid(xlin,ylin); 63 | [X,Y] = meshgrid(x,y); 64 | 65 | Z = zeros(n,n); 66 | Zlin = zeros(n,n); 67 | 68 | % Compute f(t,x) and its linearization 69 | for k=1:n 70 | for h=1:n 71 | Z(k,h) = fcn(0, [x(h),y(k)]); 72 | Zlin(k,h) = fcn(t0, x0) + dfdp*([xlin(h);ylin(k)]-x0); 73 | end 74 | end 75 | 76 | s1 = surf(X,Y,Z); 77 | set(s1, 'edgecolor','none'); 78 | 79 | s2 = surf(Xlin,Ylin,Zlin, 2*Zlin); 80 | set(s2, 'edgecolor','none'); 81 | 82 | scale = -0.5; 83 | quiver3(x0(1),x0(2),fcn(t0,x0),scale*dfdp(1),scale*dfdp(2),-scale, 'LineWidth', 2); 84 | plot3(x0(1),x0(2),fcn(t0,x0),'rx'); 85 | view(3); 86 | 87 | else 88 | % Cannot visualize a function of more than two variables 89 | disp('Dimension of domain is > 2.'); 90 | end 91 | 92 | 93 | end -------------------------------------------------------------------------------- /matlab/visualization/visualizeRotationFromAngleAxis.m: -------------------------------------------------------------------------------- 1 | function visualizeRotationFromAngleAxis(A_k_AB, theta) 2 | % VISUALIZEROTATIONFROMANGLEAXIS visualizes the effect of rotating a 3 | % coordinate A to frame B induced by a rotation represented by the 4 | % angle-axis parametrization. 5 | % 6 | % Author(s): Dario Bellicoso 7 | 8 | % Build the rotation vector from the angle-axis representation 9 | A_phi_AB = A_k_AB*theta; 10 | 11 | % Visualize the orientation 12 | visualizeRotationFromRotationVector(A_phi_AB); 13 | 14 | end -------------------------------------------------------------------------------- /matlab/visualization/visualizeRotationFromRotationVector.m: -------------------------------------------------------------------------------- 1 | function visualizeRotationFromRotationVector(A_phi_AB) 2 | % VISUALIZEROTATIONFROMROTATIONVECTOR visualizes the effect of rotating a 3 | % coordinate A to frame B induced by the rotation vector A_phi_AB. 4 | % 5 | % Author(s): Dario Bellicoso 6 | 7 | % Setup figure 8 | h = figure(); 9 | view(3); 10 | grid on; 11 | axis equal; 12 | 13 | % Map the rotation vector to the rotation matrix in SO(3) 14 | C_AB = mapRotationVectorToRotationMatrix(A_phi_AB); 15 | 16 | % Visualize the fixed and rotated frames 17 | visualizeCoordinateSystem(eye(3), h, 'A'); 18 | visualizeCoordinateSystem(C_AB, h, 'B'); 19 | 20 | % Visualize the rotation vector 21 | quiver3(0, 0, 0, A_phi_AB(1),A_phi_AB(2),A_phi_AB(3),'k'); 22 | 23 | % Label the rotation vector 24 | addLabelToVector(h, A_phi_AB, '{}_A \varphi_{AB}'); 25 | 26 | end -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | kindr 5 | 1.3.24 6 | Kinematics and Dynamics for Robotics 7 | Michael Bloesch 8 | Remo Diethelm 9 | Péter Fankhauser 10 | Paul Furgale 11 | Christian Gehring 12 | Hannes Sommer 13 | Remo Diethelm 14 | Philipp Leemann 15 | BSD 16 | https://github.com/anybotics/kindr 17 | catkin 18 | ament_cmake 19 | eigen 20 | ament_cmake_gtest 21 | gtest 22 | 23 | doxygen 24 | texlive-latex-base 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /signature_of_kindr1_library: -------------------------------------------------------------------------------- 1 | This file is just there as a signature to help identify directories containing Kindr1. When writing a script looking for Eigen1, just look for this file. This is especially useful to help disambiguate with Kindr0... 2 | 3 | -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # * Redistributions of source code must retain the above copyright 7 | # notice, this list of conditions and the following disclaimer. 8 | # * Redistributions in binary form must reproduce the above copyright 9 | # notice, this list of conditions and the following disclaimer in the 10 | # documentation and/or other materials provided with the distribution. 11 | # * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 12 | # names of its contributors may be used to endorse or promote products 13 | # derived from this software without specific prior written permission. 14 | # 15 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | # DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 19 | # Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 20 | # OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 21 | # GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 22 | # HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 23 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | # Project configuration 27 | cmake_minimum_required(VERSION 3.16.3) 28 | 29 | if("$ENV{ROS_VERSION}" STREQUAL "1" OR USE_CMAKE) 30 | 31 | add_definitions(-std=c++11) 32 | 33 | find_package(Eigen3 REQUIRED) 34 | 35 | include_directories(${EIGEN3_INCLUDE_DIR}) 36 | include_directories(../include) 37 | 38 | 39 | ################################ 40 | # Unit Tests 41 | ################################ 42 | ################################ 43 | # GTest 44 | ################################ 45 | 46 | include_directories(${gtest_SOURCE_DIR}/include ${gtest_SOURCE_DIR}) 47 | include_directories(test) 48 | 49 | # This macro globs all tests to a list so we can run them post-build. 50 | macro(_add_gtest target) 51 | set(sources ${ARGV}) 52 | LIST(REMOVE_ITEM sources ${target}) 53 | if(BUILD_CATKIN_PACKAGE AND CATKIN_ENABLE_TESTING) 54 | catkin_add_gtest(${target} ${sources}) 55 | target_include_directories(${target} PRIVATE include) 56 | target_link_libraries(${target} gtest_main) 57 | find_package(cmake_code_coverage QUIET) 58 | if(cmake_code_coverage_FOUND) 59 | add_gtest_coverage(TEST_BUILD_TARGETS ${target}) 60 | endif(cmake_code_coverage_FOUND) 61 | elseif(BUILD_GTEST) 62 | add_executable(${target} ${sources}) 63 | message("Building test for " ${sources}) 64 | set(UNIT_TEST_TARGETS ${UNIT_TEST_TARGETS} ${target}) 65 | target_link_libraries(${target} gtest gtest_main pthread) 66 | add_test(${target} ${target}) 67 | endif() 68 | endmacro(_add_gtest) 69 | 70 | set(COMMON_SRCS 71 | test_main.cpp 72 | common/CommonTest.cpp 73 | ) 74 | _add_gtest(runUnitTestsCommon ${COMMON_SRCS}) 75 | 76 | 77 | set(LINEARALGEBRA_SRCS 78 | test_main.cpp 79 | linear_algebra/SkewMatrixFromVectorTest.cpp 80 | linear_algebra/PseudoInverseTest.cpp 81 | ) 82 | _add_gtest(runUnitTestsLinearAlgebra ${LINEARALGEBRA_SRCS}) 83 | 84 | set(QUATERNIONS_SRCS 85 | test_main.cpp 86 | quaternions/QuaternionTest.cpp 87 | ) 88 | _add_gtest( runUnitTestsQuaternions ${QUATERNIONS_SRCS} ) 89 | 90 | set(ROTATION_SRCS 91 | test_main.cpp 92 | rotations/RotationMatrixTest.cpp 93 | rotations/RotationQuaternionTest.cpp 94 | rotations/RotationVectorTest.cpp 95 | rotations/AngleAxisTest.cpp 96 | rotations/RotationMatrixTest.cpp 97 | rotations/EulerAnglesZyxTest.cpp 98 | rotations/EulerAnglesXyzTest.cpp 99 | rotations/RotationTest.cpp 100 | rotations/ConventionTest.cpp 101 | 102 | ) 103 | _add_gtest( runUnitTestsRotation ${ROTATION_SRCS}) 104 | 105 | 106 | set(ROTATIONDIFF_SRCS 107 | test_main.cpp 108 | rotations/RotationDiffTest.cpp 109 | rotations/LocalAngularVelocityTest.cpp 110 | rotations/GlobalAngularVelocityTest.cpp 111 | rotations/RotationQuaternionDiffTest.cpp 112 | rotations/RotationMatrixDiffTest.cpp 113 | rotations/EulerAnglesZyxDiffTest.cpp 114 | rotations/EulerAnglesXyzDiffTest.cpp 115 | ) 116 | _add_gtest( runUnitTestsRotationDiff ${ROTATIONDIFF_SRCS}) 117 | 118 | set(POSITIONS_SRCS 119 | test_main.cpp 120 | 121 | ) 122 | _add_gtest( runUnitTestsPositions ${POSITIONS_SRCS}) 123 | 124 | set(POSES_SRCS 125 | test_main.cpp 126 | poses/PositionTest.cpp 127 | poses/HomogeneousTransformationTest.cpp 128 | ) 129 | _add_gtest( runUnitTestsPose ${POSES_SRCS}) 130 | 131 | set(POSESDIFF_SRCS 132 | test_main.cpp 133 | poses/PoseDiffTest.cpp 134 | poses/PositionDiffTest.cpp 135 | poses/TwistWithAngularVelocityTest.cpp 136 | ) 137 | _add_gtest( runUnitTestsPoseDiff ${POSESDIFF_SRCS}) 138 | 139 | set(VECTOR_SRCS 140 | test_main.cpp 141 | vectors/VectorsTest.cpp 142 | ) 143 | _add_gtest( runUnitTestsVector ${VECTOR_SRCS}) 144 | 145 | set(PHYS_QUANT_SRCS 146 | test_main.cpp 147 | phys_quant/ForceTest.cpp 148 | phys_quant/WrenchTest.cpp 149 | phys_quant/ScalarTest.cpp 150 | ) 151 | _add_gtest( runUnitTestsPhysQuant ${PHYS_QUANT_SRCS}) 152 | 153 | # Run all unit tests post-build. 154 | if(NOT TARGET run_tests) 155 | add_custom_target(run_tests ALL 156 | DEPENDS ${UNIT_TEST_TARGETS} 157 | ) 158 | add_custom_command(TARGET run_tests 159 | COMMENT "Running tests" 160 | POST_BUILD COMMAND ${PROJECT_SOURCE_DIR}/test/run_tests.py ${CMAKE_BINARY_DIR} ${UNIT_TEST_TARGETS} 161 | WORKING_DIRECTORY ${CMAKE_BINARY_DIR} 162 | ) 163 | endif() 164 | 165 | 166 | else() # ROS version 2 167 | 168 | 169 | find_package(ament_cmake_gtest REQUIRED) 170 | ament_add_gtest(test_${PROJECT_NAME} 171 | test_main.cpp 172 | common/CommonTest.cpp 173 | linear_algebra/SkewMatrixFromVectorTest.cpp 174 | linear_algebra/PseudoInverseTest.cpp 175 | quaternions/QuaternionTest.cpp 176 | rotations/RotationMatrixTest.cpp 177 | rotations/RotationQuaternionTest.cpp 178 | rotations/RotationVectorTest.cpp 179 | rotations/AngleAxisTest.cpp 180 | rotations/RotationMatrixTest.cpp 181 | rotations/EulerAnglesZyxTest.cpp 182 | rotations/EulerAnglesXyzTest.cpp 183 | rotations/RotationTest.cpp 184 | rotations/ConventionTest.cpp 185 | rotations/RotationDiffTest.cpp 186 | rotations/LocalAngularVelocityTest.cpp 187 | rotations/GlobalAngularVelocityTest.cpp 188 | rotations/RotationQuaternionDiffTest.cpp 189 | rotations/RotationMatrixDiffTest.cpp 190 | rotations/EulerAnglesZyxDiffTest.cpp 191 | rotations/EulerAnglesXyzDiffTest.cpp 192 | poses/PositionTest.cpp 193 | poses/HomogeneousTransformationTest.cpp 194 | poses/PoseDiffTest.cpp 195 | poses/PositionDiffTest.cpp 196 | poses/TwistWithAngularVelocityTest.cpp 197 | vectors/VectorsTest.cpp 198 | phys_quant/ForceTest.cpp 199 | phys_quant/WrenchTest.cpp 200 | phys_quant/ScalarTest.cpp 201 | ) 202 | target_include_directories(test_${PROJECT_NAME} PRIVATE "$") 203 | target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) 204 | ament_target_dependencies(test_${PROJECT_NAME} Eigen3) 205 | 206 | 207 | endif() 208 | -------------------------------------------------------------------------------- /test/common/CommonTest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 20 | * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 21 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 22 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | 30 | #include 31 | #include 32 | 33 | TEST (CommonTest, wrapPosNegPI) { 34 | 35 | using namespace kindr; 36 | double h = -1.0e-8; 37 | double angle = kindr::wrapPosNegPI(2.0*M_PI+h); 38 | EXPECT_NEAR(h , angle, 1.0e-10); 39 | 40 | double h2 = 1.0e-8; 41 | double angle2 = kindr::wrapPosNegPI(-2.0*M_PI+h2); 42 | EXPECT_NEAR(h2 , angle2, 1.0e-10); 43 | } 44 | -------------------------------------------------------------------------------- /test/linear_algebra/PseudoInverseTest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 20 | * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 21 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 22 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | TEST (SquarePseudoInverseTest, testVector) { 35 | 36 | Eigen::Matrix2d A; 37 | A << 61.6806, 345.8256, 38 | 176.1109, 605.4883; 39 | 40 | Eigen::Matrix2d expectedPinvA; 41 | expectedPinvA << -0.0257, 0.0147, 42 | 0.0075, -0.0026; 43 | 44 | Eigen::Matrix2d pinvA; 45 | kindr::pseudoInverse(A, pinvA); 46 | 47 | KINDR_ASSERT_DOUBLE_MX_EQ_ABS_REL(pinvA, expectedPinvA, 2e-3, 0, ""); 48 | } 49 | 50 | 51 | TEST (RowPseudoInverseTest, testVector) { 52 | 53 | Eigen::Matrix A; 54 | A << 0.3862, 0.2766, 55 | 0.3341, 0.0151, 56 | 0.4047, 0.3581; 57 | Eigen::MatrixXd Ad = A; 58 | 59 | Eigen::Matrix expectedPinvA; 60 | expectedPinvA << 0.3864, 3.0637, -0.4272, 61 | 0.8652, -3.7645, 2.2824; 62 | Eigen::MatrixXd expectedPinvAd = expectedPinvA; 63 | 64 | Eigen::Matrix pinvA; 65 | Eigen::MatrixXd pinvAd; 66 | 67 | kindr::pseudoInverse(A, pinvA); 68 | kindr::pseudoInverse(Ad, pinvAd); 69 | 70 | KINDR_ASSERT_DOUBLE_MX_EQ_ABS_REL(pinvA, expectedPinvA, 2e-3, 0, ""); 71 | KINDR_ASSERT_DOUBLE_MX_EQ_ABS_REL(pinvAd, expectedPinvAd, 2e-3, 0, ""); 72 | } 73 | 74 | TEST (ColPseudoInverseTest, testVector) { 75 | Eigen::Matrix A; 76 | A << 0.6339, 0.6941, 0.6122, 77 | 0.7077, 0.3663, 0.1599; 78 | Eigen::MatrixXd Ad = A; 79 | 80 | Eigen::Matrix expectedPinvA; 81 | expectedPinvA << -0.7783, 2.0147, 82 | 0.8690, -0.4988, 83 | 1.4542, -1.5207; 84 | Eigen::MatrixXd expectedPinvAd = expectedPinvA; 85 | 86 | Eigen::Matrix pinvA; 87 | Eigen::MatrixXd pinvAd; 88 | 89 | kindr::pseudoInverse(A, pinvA); 90 | kindr::pseudoInverse(Ad, pinvAd); 91 | 92 | KINDR_ASSERT_DOUBLE_MX_EQ_ABS_REL(pinvA, expectedPinvA, 2e-3, 0, ""); 93 | KINDR_ASSERT_DOUBLE_MX_EQ_ABS_REL(pinvAd, expectedPinvAd, 2e-3, 0, ""); 94 | } 95 | -------------------------------------------------------------------------------- /test/linear_algebra/SkewMatrixFromVectorTest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 20 | * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 21 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 22 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | 30 | #include 31 | #include 32 | 33 | TEST (SkewMatrixFromVectorTest, testVector) { 34 | Eigen::Matrix3d skewMatrix; 35 | Eigen::Vector3d vec; 36 | vec << 1, 2, 3; 37 | skewMatrix << 0, -3, 2, 3, 0, -1, -2, 1, 0; 38 | EXPECT_EQ(skewMatrix, kindr::getSkewMatrixFromVector(vec)); 39 | } 40 | -------------------------------------------------------------------------------- /test/phys_quant/ForceTest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 20 | * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 21 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 22 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #include 30 | 31 | #include 32 | 33 | #include 34 | 35 | #include "kindr/phys_quant/PhysicalQuantities.hpp" 36 | #include "kindr/common/gtest_eigen.hpp" 37 | 38 | namespace vectors = kindr; 39 | 40 | template 41 | struct ForceTorqueTest: public ::testing::Test { 42 | typedef ForceImplementation Force; 43 | typedef typename Force::Scalar Scalar; 44 | // const int length = Force::Dimension; 45 | typedef typename Force::Implementation EigenVector; 46 | 47 | Scalar tol; 48 | EigenVector vecZero, vec1, vec2, vecAdd, vecSubtract; 49 | 50 | Force forceDefault; 51 | Force vectorFromMultipleValues; 52 | Force forceFromEigen; 53 | Force force2FromEigen; 54 | Force forceFromForce; 55 | 56 | ForceTorqueTest() : tol(1e-6) 57 | { 58 | 59 | } 60 | }; 61 | 62 | 63 | typedef ::testing::Types< 64 | vectors::VectorTypeless3D 65 | > Types3; 66 | 67 | 68 | TYPED_TEST_CASE(ForceTorqueTest, Types3); 69 | 70 | TYPED_TEST(ForceTorqueTest, testForce) 71 | { 72 | typedef vectors::VectorTypeless3D Vector; 73 | typedef vectors::Force3D Force; 74 | typedef vectors::Torque3D Torque; 75 | 76 | Vector v(1,2,3); 77 | Force f1(v); 78 | Torque t1(v); 79 | 80 | // Force f2 = t1 + f1; 81 | std::cout << f1 << std::endl; 82 | f1 = Force::Zero(); 83 | std::cout << f1 << std::endl; 84 | } 85 | 86 | -------------------------------------------------------------------------------- /test/phys_quant/ScalarTest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "kindr/common/gtest_eigen.hpp" 4 | #include "kindr/phys_quant/PhysicalType.hpp" 5 | #include "kindr/phys_quant/PhysicalQuantities.hpp" 6 | #include "kindr/vectors/Vector.hpp" 7 | 8 | struct ScalarTest : public ::testing::Test { 9 | using Acceleration = kindr::Acceleration3D; 10 | using Force = kindr::Force3D; 11 | using Mass = kindr::MassD; 12 | 13 | Acceleration acceleration; 14 | Force force; 15 | Mass mass; 16 | 17 | ScalarTest() : acceleration(1., 2., 3.), force(5., 10., 15.), mass(5.) {} 18 | }; 19 | 20 | TEST_F(ScalarTest, multiplicationWithScalar) { 21 | EXPECT_EQ(acceleration * mass, force); 22 | EXPECT_EQ(mass * acceleration, force); 23 | } 24 | 25 | TEST_F(ScalarTest, divisionByScalar) { 26 | EXPECT_EQ(force / mass, acceleration); 27 | } 28 | 29 | TEST_F(ScalarTest, multiplyTwoScalars) { 30 | kindr::Acceleration accelerationScalar(2.); 31 | kindr::Force forceScalar(10.); 32 | 33 | EXPECT_EQ(accelerationScalar * mass, forceScalar); 34 | EXPECT_EQ(mass * accelerationScalar, forceScalar); 35 | } 36 | 37 | TEST_F(ScalarTest, divideTwoScalars) { 38 | kindr::Acceleration accelerationScalar(2.); 39 | kindr::Force forceScalar(10.); 40 | 41 | EXPECT_EQ(forceScalar / mass, accelerationScalar); 42 | EXPECT_EQ(forceScalar / accelerationScalar, mass); 43 | } 44 | 45 | TEST_F(ScalarTest, assignment) { 46 | const kindr::VectorTypeless typeless(5.); 47 | 48 | acceleration *= typeless; 49 | EXPECT_EQ(acceleration, kindr::Acceleration3D(5., 10., 15.)); 50 | 51 | force /= typeless; 52 | EXPECT_EQ(force, kindr::Force3D(1., 2., 3.)); 53 | } 54 | -------------------------------------------------------------------------------- /test/poses/PoseDiffTest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 20 | * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 21 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 22 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #include 30 | 31 | #include 32 | 33 | #include 34 | 35 | #include "kindr/poses/PoseDiff.hpp" 36 | #include "kindr/poses/PoseDiffBase.hpp" 37 | #include "kindr/phys_quant/PhysicalQuantities.hpp" 38 | 39 | namespace pose = kindr; 40 | namespace pos = kindr; 41 | namespace rot = kindr; 42 | 43 | 44 | typedef ::testing::Types< 45 | pose::TwistLinearVelocityRotationQuaternionDiffD, 46 | pose::TwistLinearVelocityRotationQuaternionDiffF 47 | > Types; 48 | 49 | 50 | template 51 | struct TwistTest: public ::testing::Test { 52 | typedef PoseImplementation PoseDiff; 53 | }; 54 | 55 | TYPED_TEST_CASE(TwistTest, Types); 56 | 57 | TYPED_TEST(TwistTest, testConstructor) 58 | { 59 | 60 | } 61 | 62 | TYPED_TEST(TwistTest, testInitial) 63 | { 64 | typedef typename TestFixture::PoseDiff PoseDiff; 65 | typedef typename TestFixture::PoseDiff::Scalar Scalar; 66 | typedef rot::RotationQuaternion Rotation; 67 | 68 | typename PoseDiff::PositionDiff linearVelocity(0.1, 0.2, 0.3); 69 | rot::LocalAngularVelocityD angularVelocity; 70 | typename PoseDiff::RotationDiff rquatDiff(1, 2, 3, 4); 71 | PoseDiff twist(linearVelocity, rquatDiff); 72 | std::cout << twist << std::endl; 73 | 74 | Rotation rot; 75 | std::cout << twist.getVector(rot) << std::endl; 76 | } 77 | 78 | TYPED_TEST(TwistTest, testSetZero) 79 | { 80 | typedef typename TestFixture::PoseDiff PoseDiff; 81 | 82 | PoseDiff twist(typename PoseDiff::PositionDiff(0.1, 0.2, 0.3), typename PoseDiff::RotationDiff(1, 2, 3, 4)); 83 | 84 | twist.setZero(); 85 | 86 | ASSERT_EQ(twist.getTranslationalVelocity().vector().x(), 0); 87 | ASSERT_EQ(twist.getTranslationalVelocity().vector().y(), 0); 88 | ASSERT_EQ(twist.getTranslationalVelocity().vector().z(), 0); 89 | 90 | ASSERT_EQ(twist.getRotationalVelocity().w(), 0); 91 | ASSERT_EQ(twist.getRotationalVelocity().x(), 0); 92 | ASSERT_EQ(twist.getRotationalVelocity().y(), 0); 93 | ASSERT_EQ(twist.getRotationalVelocity().z(), 0); 94 | } 95 | 96 | 97 | -------------------------------------------------------------------------------- /test/poses/PositionTest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 20 | * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 21 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 22 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #include 30 | 31 | #include 32 | 33 | #include 34 | 35 | #include "kindr/phys_quant/PhysicalQuantities.hpp" 36 | #include "kindr/common/gtest_eigen.hpp" 37 | 38 | 39 | 40 | namespace pos = kindr; 41 | 42 | template 43 | struct Position3Test: public ::testing::Test { 44 | typedef PositionImplementation Pos3; 45 | typedef typename Pos3::Scalar Scalar; 46 | typedef Eigen::Matrix Vector3; 47 | 48 | Scalar tol; 49 | Scalar factor; 50 | Scalar divisor; 51 | Vector3 vecZero, vec1, vec2, vecAdd, vecSubtract, vecMult, vecDiv; 52 | 53 | Pos3 posDefault; 54 | Pos3 posFromThreeValues; 55 | Pos3 posFromEigen; 56 | Pos3 pos2FromEigen; 57 | Pos3 posFromPos; 58 | 59 | Position3Test() : tol(1e-6), 60 | factor(2), 61 | divisor(2), 62 | vecZero(Vector3::Zero()), 63 | vec1(10,20,30), 64 | vec2(1,2,3), 65 | vecAdd(11,22,33), 66 | vecSubtract(9,18,27), 67 | vecMult(20,40,60), 68 | vecDiv(5,10,15), 69 | posFromThreeValues(vec1.x(), vec1.y(), vec1.z()), 70 | posFromEigen(vec1), 71 | pos2FromEigen(vec2), 72 | posFromPos(posFromEigen) 73 | {} 74 | }; 75 | 76 | 77 | typedef ::testing::Types< 78 | pos::Position3D, 79 | pos::Position3F 80 | > Types; 81 | 82 | 83 | TYPED_TEST_CASE(Position3Test, Types); 84 | 85 | TYPED_TEST(Position3Test, testPosition3) 86 | { 87 | typedef typename TestFixture::Pos3 Pos3; 88 | 89 | // default constructor 90 | ASSERT_EQ(this->posDefault.x(), this->vecZero.x()) << "Default constructor needs to initialize x-component to zero!"; 91 | ASSERT_EQ(this->posDefault.y(), this->vecZero.y()) << "Default constructor needs to initialize y-component to zero!"; 92 | ASSERT_EQ(this->posDefault.z(), this->vecZero.z()) << "Default constructor needs to initialize z-component to zero!"; 93 | 94 | // constructor with three values (x,y,z) 95 | ASSERT_EQ(this->posFromThreeValues.x(), this->vec1.x()) << "Three-Value Constructor needs to first initialize x-component!"; 96 | ASSERT_EQ(this->posFromThreeValues.y(), this->vec1.y()) << "Three-Value Constructor needs to second initialize y-component!"; 97 | ASSERT_EQ(this->posFromThreeValues.z(), this->vec1.z()) << "Three-Value Constructor needs to third initialize z-component!"; 98 | 99 | // constructor with Eigen vector 100 | ASSERT_EQ(this->posFromEigen.x(), this->vec1.x()) << "Base Constructor needs to first initialize x-component!"; 101 | ASSERT_EQ(this->posFromEigen.y(), this->vec1.y()) << "Base Constructor needs to second initialize y-component!"; 102 | ASSERT_EQ(this->posFromEigen.z(), this->vec1.z()) << "Base Constructor needs to third initialize z-component!"; 103 | 104 | // constructor with Position3 105 | ASSERT_EQ(this->posFromPos.x(), this->vec1.x()); 106 | ASSERT_EQ(this->posFromPos.y(), this->vec1.y()); 107 | ASSERT_EQ(this->posFromPos.z(), this->vec1.z()); 108 | 109 | // toImplementation 110 | ASSERT_EQ(this->posFromThreeValues.toImplementation()(0,0), this->vec1.x()) << "X-component needs to correspond to the matrix entry (0,0)!"; 111 | ASSERT_EQ(this->posFromThreeValues.toImplementation()(1,0), this->vec1.y()) << "Y-component needs to correspond to the matrix entry (1,0)!"; 112 | ASSERT_EQ(this->posFromThreeValues.toImplementation()(2,0), this->vec1.z()) << "Z-component needs to correspond to the matrix entry (2,0)!"; 113 | 114 | // addition 115 | Pos3 posAdd = this->posFromEigen+this->pos2FromEigen; 116 | ASSERT_EQ(posAdd.x(), this->vecAdd.x()); 117 | ASSERT_EQ(posAdd.y(), this->vecAdd.y()); 118 | ASSERT_EQ(posAdd.z(), this->vecAdd.z()); 119 | 120 | // addition and assignment 121 | Pos3 posAddandAssign(this->posFromEigen); 122 | posAddandAssign += this->pos2FromEigen; 123 | ASSERT_EQ(posAddandAssign.x(), this->vecAdd.x()); 124 | ASSERT_EQ(posAddandAssign.y(), this->vecAdd.y()); 125 | ASSERT_EQ(posAddandAssign.z(), this->vecAdd.z()); 126 | 127 | // subtraction 128 | Pos3 posSubtract = this->posFromEigen-this->pos2FromEigen; 129 | ASSERT_EQ(posSubtract.x(), this->vecSubtract.x()); 130 | ASSERT_EQ(posSubtract.y(), this->vecSubtract.y()); 131 | ASSERT_EQ(posSubtract.z(), this->vecSubtract.z()); 132 | 133 | // subtraction and assignment 134 | Pos3 posSubtractandAssign(this->posFromEigen); 135 | posSubtractandAssign -= this->pos2FromEigen; 136 | ASSERT_EQ(posSubtractandAssign.x(), this->vecSubtract.x()); 137 | ASSERT_EQ(posSubtractandAssign.y(), this->vecSubtract.y()); 138 | ASSERT_EQ(posSubtractandAssign.z(), this->vecSubtract.z()); 139 | 140 | // multiplication a) 141 | Pos3 posMultA = this->posFromEigen * this->factor; 142 | ASSERT_EQ(posMultA.x(), this->vecMult.x()); 143 | ASSERT_EQ(posMultA.y(), this->vecMult.y()); 144 | ASSERT_EQ(posMultA.z(), this->vecMult.z()); 145 | 146 | // multiplication b) 147 | Pos3 posMultB = this->factor * this->posFromEigen; 148 | ASSERT_EQ(posMultB.x(), this->vecMult.x()); 149 | ASSERT_EQ(posMultB.y(), this->vecMult.y()); 150 | ASSERT_EQ(posMultB.z(), this->vecMult.z()); 151 | 152 | // multiplication and assignment 153 | Pos3 posMultAndAssign(this->posFromEigen); 154 | posMultAndAssign *= this->factor; 155 | ASSERT_EQ(posMultAndAssign.x(), this->vecMult.x()); 156 | ASSERT_EQ(posMultAndAssign.y(), this->vecMult.y()); 157 | ASSERT_EQ(posMultAndAssign.z(), this->vecMult.z()); 158 | 159 | // division 160 | Pos3 posDiv = this->posFromEigen / this->divisor; 161 | ASSERT_EQ(posDiv.x(), this->vecDiv.x()); 162 | ASSERT_EQ(posDiv.y(), this->vecDiv.y()); 163 | ASSERT_EQ(posDiv.z(), this->vecDiv.z()); 164 | 165 | // division and assignment 166 | Pos3 posDivAndAssign(this->posFromEigen); 167 | posDivAndAssign /= this->divisor; 168 | ASSERT_EQ(posDivAndAssign.x(), this->vecDiv.x()); 169 | ASSERT_EQ(posDivAndAssign.y(), this->vecDiv.y()); 170 | ASSERT_EQ(posDivAndAssign.z(), this->vecDiv.z()); 171 | } 172 | 173 | -------------------------------------------------------------------------------- /test/poses/TwistWithAngularVelocityTest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 20 | * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 21 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 22 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #include 30 | 31 | #include 32 | 33 | #include 34 | 35 | #include 36 | 37 | 38 | typedef ::testing::Types< 39 | kindr::TwistLocalD, 40 | kindr::TwistLocalF, 41 | kindr::TwistGlobalD, 42 | kindr::TwistGlobalF 43 | > TwistTypes; 44 | 45 | template 46 | struct TwistWithAngularVelocityTest: public ::testing::Test { 47 | typedef Twist_ Twist; 48 | typedef typename Twist::Scalar Scalar; 49 | typedef typename Twist::PositionDiff PositionDiff; 50 | typedef typename Twist::RotationDiff RotationDiff; 51 | }; 52 | 53 | TYPED_TEST_CASE(TwistWithAngularVelocityTest, TwistTypes); 54 | 55 | TYPED_TEST(TwistWithAngularVelocityTest, constructors) 56 | { 57 | typedef typename TestFixture::Twist Twist; 58 | typedef typename TestFixture::PositionDiff PositionDiff; 59 | typedef typename TestFixture::RotationDiff RotationDiff; 60 | typedef typename TestFixture::Scalar Scalar; 61 | 62 | // Default constructor 63 | Twist twist1; 64 | EXPECT_EQ(0.0, twist1.getTranslationalVelocity().x()); 65 | EXPECT_EQ(0.0, twist1.getTranslationalVelocity().y()); 66 | EXPECT_EQ(0.0, twist1.getTranslationalVelocity().z()); 67 | EXPECT_EQ(0.0, twist1.getRotationalVelocity().x()); 68 | EXPECT_EQ(0.0, twist1.getRotationalVelocity().y()); 69 | EXPECT_EQ(0.0, twist1.getRotationalVelocity().z()); 70 | 71 | // Copy constructor 72 | Twist twist2(PositionDiff(1.0, 2.0, 3.0), RotationDiff(5.0, 6.0, 7.0)); 73 | Twist twist3(twist2); 74 | EXPECT_EQ(1.0, twist3.getTranslationalVelocity().x()); 75 | EXPECT_EQ(2.0, twist3.getTranslationalVelocity().y()); 76 | EXPECT_EQ(3.0, twist3.getTranslationalVelocity().z()); 77 | EXPECT_EQ(5.0, twist3.getRotationalVelocity().x()); 78 | EXPECT_EQ(6.0, twist3.getRotationalVelocity().y()); 79 | EXPECT_EQ(7.0, twist3.getRotationalVelocity().z()); 80 | EXPECT_EQ(1.0, twist3.getVector()(0)); 81 | EXPECT_EQ(2.0, twist3.getVector()(1)); 82 | EXPECT_EQ(3.0, twist3.getVector()(2)); 83 | EXPECT_EQ(5.0, twist3.getVector()(3)); 84 | EXPECT_EQ(6.0, twist3.getVector()(4)); 85 | EXPECT_EQ(7.0, twist3.getVector()(5)); 86 | 87 | 88 | 89 | // Vector6 90 | typename Twist::Vector6 vector6; 91 | vector6 << 1.0, 2.0, 3.0, 5.0, 6.0, 7.0; 92 | Twist twist4(vector6); 93 | EXPECT_EQ(1.0, twist4.getTranslationalVelocity().x()); 94 | EXPECT_EQ(2.0, twist4.getTranslationalVelocity().y()); 95 | EXPECT_EQ(3.0, twist4.getTranslationalVelocity().z()); 96 | EXPECT_EQ(5.0, twist4.getRotationalVelocity().x()); 97 | EXPECT_EQ(6.0, twist4.getRotationalVelocity().y()); 98 | EXPECT_EQ(7.0, twist4.getRotationalVelocity().z()); 99 | 100 | // 2xVector3 101 | typename Twist::Vector3 linVel(1.0, 2.0, 3.0); 102 | typename Twist::Vector3 angVel(5.0, 6.0, 7.0); 103 | Twist twist5(linVel, angVel); 104 | EXPECT_EQ(1.0, twist5.getTranslationalVelocity().x()); 105 | EXPECT_EQ(2.0, twist5.getTranslationalVelocity().y()); 106 | EXPECT_EQ(3.0, twist5.getTranslationalVelocity().z()); 107 | EXPECT_EQ(5.0, twist5.getRotationalVelocity().x()); 108 | EXPECT_EQ(6.0, twist5.getRotationalVelocity().y()); 109 | EXPECT_EQ(7.0, twist5.getRotationalVelocity().z()); 110 | 111 | } 112 | 113 | 114 | TYPED_TEST(TwistWithAngularVelocityTest, getters) 115 | { 116 | typedef typename TestFixture::Twist Twist; 117 | typedef typename TestFixture::PositionDiff PositionDiff; 118 | typedef typename TestFixture::RotationDiff RotationDiff; 119 | typedef typename TestFixture::Scalar Scalar; 120 | 121 | Twist twist1(PositionDiff(1.0, 2.0, 3.0), RotationDiff(5.0, 6.0, 7.0)); 122 | 123 | EXPECT_EQ(1.0, twist1.getTranslationalVelocity().x()); 124 | EXPECT_EQ(2.0, twist1.getTranslationalVelocity().y()); 125 | EXPECT_EQ(3.0, twist1.getTranslationalVelocity().z()); 126 | EXPECT_EQ(5.0, twist1.getRotationalVelocity().x()); 127 | EXPECT_EQ(6.0, twist1.getRotationalVelocity().y()); 128 | EXPECT_EQ(7.0, twist1.getRotationalVelocity().z()); 129 | EXPECT_EQ(1.0, twist1.getVector()(0)); 130 | EXPECT_EQ(2.0, twist1.getVector()(1)); 131 | EXPECT_EQ(3.0, twist1.getVector()(2)); 132 | EXPECT_EQ(5.0, twist1.getVector()(3)); 133 | EXPECT_EQ(6.0, twist1.getVector()(4)); 134 | EXPECT_EQ(7.0, twist1.getVector()(5)); 135 | 136 | } 137 | 138 | TYPED_TEST(TwistWithAngularVelocityTest, setters) 139 | { 140 | typedef typename TestFixture::Twist Twist; 141 | typedef typename TestFixture::PositionDiff PositionDiff; 142 | typedef typename TestFixture::RotationDiff RotationDiff; 143 | typedef typename TestFixture::Scalar Scalar; 144 | 145 | Twist twist1; 146 | twist1.getTranslationalVelocity() = PositionDiff(1.0, 2.0, 3.0); 147 | twist1.getRotationalVelocity() = RotationDiff(5.0, 6.0, 7.0); 148 | EXPECT_EQ(1.0, twist1.getTranslationalVelocity().x()); 149 | EXPECT_EQ(2.0, twist1.getTranslationalVelocity().y()); 150 | EXPECT_EQ(3.0, twist1.getTranslationalVelocity().z()); 151 | EXPECT_EQ(5.0, twist1.getRotationalVelocity().x()); 152 | EXPECT_EQ(6.0, twist1.getRotationalVelocity().y()); 153 | EXPECT_EQ(7.0, twist1.getRotationalVelocity().z()); 154 | 155 | Twist twist2; 156 | typename Twist::Vector6 vector6; 157 | vector6 << 1.0, 2.0, 3.0, 5.0, 6.0, 7.0; 158 | twist2.setVector(vector6); 159 | EXPECT_EQ(1.0, twist2.getTranslationalVelocity().x()); 160 | EXPECT_EQ(2.0, twist2.getTranslationalVelocity().y()); 161 | EXPECT_EQ(3.0, twist2.getTranslationalVelocity().z()); 162 | EXPECT_EQ(5.0, twist2.getRotationalVelocity().x()); 163 | EXPECT_EQ(6.0, twist2.getRotationalVelocity().y()); 164 | EXPECT_EQ(7.0, twist2.getRotationalVelocity().z()); 165 | } 166 | -------------------------------------------------------------------------------- /test/rotations/ConventionTest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * convention.cpp 3 | * 4 | * Created on: Jun 17, 2016 5 | * Author: gech 6 | */ 7 | #include 8 | #include 9 | #include 10 | 11 | namespace kindr { 12 | 13 | 14 | template 15 | class RotationConversion, typename Rotation_::Scalar> { 16 | typedef typename Rotation_::Scalar Scalar; 17 | typedef Rotation_ Rotation; 18 | typedef Eigen::Matrix Vector; 19 | public: 20 | inline static void convertToOtherRotation(Rotation& otherRotation, const kindr::RotationQuaternion& quaternionIn) { 21 | // Implement quaternionOut = f(quaternionIn); 22 | otherRotation = Rotation(quaternionIn); 23 | } 24 | 25 | inline static void convertToKindr(kindr::RotationQuaternion& quaternion, const Rotation& otherRotation) { 26 | // Implement quaternionOut = f(quaternionIn); 27 | quaternion = kindr::RotationQuaternion(otherRotation); 28 | } 29 | 30 | inline static void convertToOtherVelocityVector(Vector& velocityOut, const Rotation& rot, const Eigen::Matrix& velocityIn) { 31 | // Implement velocityOut = g(velocityIn, rot); 32 | velocityOut = velocityIn; 33 | } 34 | 35 | inline static void getRotationMatrixFromRotation(Eigen::Matrix& rotationMatrix, const Rotation& rotation) { 36 | // Implement rotationMatrix = C(quaternion); 37 | rotationMatrix = kindr::RotationMatrix(rotation).matrix(); 38 | } 39 | 40 | inline static void concatenate(Rotation& res, const Rotation& rot1, const Rotation& rot2) { 41 | res = rot2*rot1; 42 | } 43 | 44 | inline static void rotateVector(Eigen::Matrix& A_r, const Rotation& rotation, const Eigen::Matrix& B_r) { 45 | A_r = rotation.rotate(B_r); 46 | } 47 | 48 | inline static void boxPlus(Rotation& res, const Rotation& rotation, const Vector& velocity) { 49 | // Implement res = rotation.boxPlus(vector); 50 | res = rotation.boxPlus(velocity); 51 | } 52 | 53 | inline static bool testRotation(const Rotation& expected, const Rotation& actual) { 54 | // Implement EXPECT_NEAR(expected, actual, 1.0e-6); 55 | return expected.isNear(actual, 1.0e-3); 56 | } 57 | }; 58 | 59 | } 60 | 61 | template 62 | struct KindrConventionTest : public ::testing::Test{ 63 | typedef typename Rotation_::Scalar Scalar; 64 | typedef Rotation_ Rotation; 65 | }; 66 | 67 | typedef ::testing::Types< 68 | kindr::RotationQuaternionD, 69 | kindr::EulerAnglesZyxD, 70 | kindr::EulerAnglesXyzD, 71 | kindr::RotationMatrixD, 72 | kindr::RotationVectorD, 73 | kindr::AngleAxisD 74 | > Types; 75 | 76 | TYPED_TEST_CASE(KindrConventionTest, Types); 77 | 78 | TYPED_TEST(KindrConventionTest, GeometricalInterpretation) 79 | { 80 | typedef typename TestFixture::Scalar Scalar; 81 | typedef typename TestFixture::Rotation Rotation; 82 | kindr::ConventionTest, Scalar>::testGeometricalInterpretation(); 83 | } 84 | 85 | TYPED_TEST(KindrConventionTest, Concatenation) 86 | { 87 | typedef typename TestFixture::Scalar Scalar; 88 | typedef typename TestFixture::Rotation Rotation; 89 | kindr::ConventionTest, Scalar>::testConcatenation(); 90 | } 91 | 92 | TYPED_TEST(KindrConventionTest, BoxPlus) 93 | { 94 | typedef typename TestFixture::Scalar Scalar; 95 | typedef typename TestFixture::Rotation Rotation; 96 | kindr::ConventionTest, Scalar>::testBoxPlus(); 97 | } 98 | 99 | TYPED_TEST(KindrConventionTest, RotationMatrix) 100 | { 101 | typedef typename TestFixture::Scalar Scalar; 102 | typedef typename TestFixture::Rotation Rotation; 103 | kindr::ConventionTest, Scalar>::testRotationMatrix(); 104 | } 105 | 106 | 107 | -------------------------------------------------------------------------------- /test/run_tests.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | 4 | import os 5 | import sys 6 | import subprocess 7 | 8 | def main(argv=sys.argv[1:]): 9 | working_dir = argv[0] 10 | tests = argv[1:] 11 | print("Working dir: {}".format(working_dir)) 12 | print("Running tests: ", tests) 13 | 14 | test_dir = working_dir + "/test_results" 15 | if not os.path.exists(test_dir): 16 | os.makedirs(test_dir) 17 | 18 | for the_file in os.listdir(test_dir): 19 | file_path = os.path.join(test_dir, the_file) 20 | try: 21 | if os.path.isfile(file_path): 22 | os.unlink(file_path) 23 | except Exception as e: 24 | print(e) 25 | 26 | commands = [] 27 | 28 | for cmd in tests: 29 | test_cmd = "./" + cmd + " --gtest_output=xml:" + working_dir + "/test_results/" 30 | print("Running: ", test_cmd) 31 | rc = subprocess.call(test_cmd, cwd=working_dir, shell=True) 32 | 33 | commands.insert(0, test_cmd) 34 | 35 | print("Ran the following commands: ") 36 | for cmd in commands: 37 | print("-- ", cmd) 38 | 39 | if __name__ == '__main__': 40 | sys.exit(main()) 41 | -------------------------------------------------------------------------------- /test/test_main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale, 20 | * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 21 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 22 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | #include 29 | 30 | 31 | /// Run all the tests that were declared with TEST() 32 | int main(int argc, char **argv){ 33 | testing::InitGoogleTest(&argc, argv); 34 | return RUN_ALL_TESTS(); 35 | } 36 | --------------------------------------------------------------------------------