├── .gitattributes ├── .github ├── CODEOWNERS └── workflows │ └── humble-source-build.yaml ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── doc ├── images │ ├── .gitattributes │ └── axebot-kinematic-frames.jpg └── kinematics_and_odometry.md ├── include └── omnidirectional_controllers │ ├── kinematics.hpp │ ├── odometry.hpp │ ├── omnidirectional_controller.hpp │ └── types.hpp ├── omnidirectional_plugin.xml ├── package.xml ├── src ├── kinematics.cpp ├── odometry.cpp └── omnidirectional_controller.cpp └── test ├── test_kinematics.cpp └── test_odometry.cpp /.gitattributes: -------------------------------------------------------------------------------- 1 | *.png filter=lfs diff=lfs merge=lfs -text 2 | -------------------------------------------------------------------------------- /.github/CODEOWNERS: -------------------------------------------------------------------------------- 1 | # These owners will be the default owners for everything in 2 | # the repo. Unless a later match takes precedence, 3 | # @global-owner1 and @global-owner2 will be requested for 4 | # review when someone opens a pull request. 5 | * @mateusmenezes95 6 | -------------------------------------------------------------------------------- /.github/workflows/humble-source-build.yaml: -------------------------------------------------------------------------------- 1 | name: Humble Source Build 2 | 3 | on: 4 | pull_request: 5 | branches: 6 | - humble 7 | 8 | jobs: 9 | humble_source_build: 10 | runs-on: ubuntu-22.04 11 | steps: 12 | - uses: ros-tooling/setup-ros@v0.7 13 | with: 14 | rosdistro: humble 15 | - uses: ros-tooling/action-ros-ci@v0.3 16 | with: 17 | package-name: omnidirectional_controllers 18 | ref: ${{ github.event.pull_request.head.sha }} 19 | target-ros2-distro: humble 20 | skip-tests: true 21 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | CATKIN_IGNORE 52 | 53 | 54 | # vscode folder 55 | .vscode 56 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(omnidirectional_controllers) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | 21 | set(THIS_PACKAGE_INCLUDE_DEPENDS 22 | controller_interface 23 | geometry_msgs 24 | hardware_interface 25 | nav_msgs 26 | pluginlib 27 | rclcpp_lifecycle 28 | rclcpp 29 | tf2_msgs 30 | tf2_ros 31 | tf2 32 | ) 33 | 34 | find_package(ament_cmake REQUIRED) 35 | foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) 36 | find_package(${Dependency} REQUIRED) 37 | endforeach() 38 | 39 | add_library(odometry SHARED) 40 | 41 | target_sources(odometry 42 | PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/odometry.cpp 43 | ) 44 | 45 | target_include_directories(odometry 46 | PUBLIC $ 47 | $ 48 | ) 49 | 50 | add_library(kinematics SHARED) 51 | 52 | target_sources(kinematics 53 | PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/kinematics.cpp 54 | ) 55 | 56 | target_include_directories(kinematics 57 | PUBLIC $ 58 | $ 59 | ) 60 | 61 | add_library(${PROJECT_NAME} SHARED 62 | src/omnidirectional_controller.cpp 63 | ) 64 | 65 | target_include_directories(${PROJECT_NAME} 66 | PUBLIC $ 67 | $ 68 | ) 69 | 70 | ament_target_dependencies(${PROJECT_NAME} 71 | ${THIS_PACKAGE_INCLUDE_DEPENDS} 72 | ) 73 | 74 | target_link_libraries(${PROJECT_NAME} 75 | odometry 76 | kinematics 77 | ) 78 | 79 | target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 80 | pluginlib_export_plugin_description_file(controller_interface omnidirectional_plugin.xml) 81 | 82 | install(DIRECTORY include/ 83 | DESTINATION include 84 | ) 85 | 86 | install(TARGETS ${PROJECT_NAME} 87 | RUNTIME DESTINATION bin 88 | ARCHIVE DESTINATION lib 89 | LIBRARY DESTINATION lib 90 | ) 91 | 92 | if(BUILD_TESTING) 93 | find_package(ament_lint_auto REQUIRED) 94 | find_package(ament_cmake_gmock REQUIRED) 95 | 96 | ament_lint_auto_find_test_dependencies() 97 | 98 | ament_add_gmock( 99 | test_odometry 100 | test/test_odometry.cpp 101 | src/odometry.cpp 102 | ) 103 | 104 | target_link_libraries(test_odometry 105 | odometry 106 | kinematics 107 | ) 108 | 109 | ament_add_gmock( 110 | test_kinematics 111 | test/test_kinematics.cpp 112 | src/kinematics.cpp 113 | ) 114 | 115 | target_link_libraries(test_kinematics 116 | odometry 117 | kinematics 118 | ) 119 | 120 | target_include_directories(test_odometry PRIVATE include) 121 | ament_target_dependencies(test_odometry) 122 | 123 | target_include_directories(test_kinematics PRIVATE include) 124 | ament_target_dependencies(test_kinematics) 125 | endif() 126 | 127 | ament_export_dependencies( 128 | ${THIS_PACKAGE_INCLUDE_DEPENDS} 129 | ) 130 | 131 | ament_export_include_directories( 132 | include 133 | ) 134 | 135 | ament_export_libraries( 136 | ${PROJECT_NAME} 137 | ) 138 | 139 | ament_package() 140 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Mateus Menezes 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Omnidirectional Controllers 2 | 3 | This package provides ROS 2 controllers for controlling Omnidirectional robots with three wheels. It is based on the concepts of [ros2_control] and [ros2_controllers]. Initially, only forward and inverse kinematics, based solely on the [diff_drive_controller], and odometry calculation have been implemented. The input for control is robot body velocity ($\dot{x}$, $\dot{y}$, $\dot{\theta}$) commands, which are translated into wheel commands ($\omega_1$, $\omega_2$, $\omega_3$) for an omnidirectional robot with three wheels. Odometry is computed from hardware feedback and published. It is worth noting that there are plans to further develop advanced linear and non-linear controllers, such as Model Predictive Control (MPC). 4 | 5 | See the documentation [Omnidirectional Robot Kinematics and Odometry](doc/kinematics_and_odometry.md) for more details. 6 | 7 | **Author:** Mateus Menezes
8 | **Maintainer:** Mateus Menezes, mateusmenezes95@gmail.com 9 | 10 | ## Build status 11 | 12 | ROS2 Distro | Branch | Build status | 13 | :---------: | :----: | :----------: | 14 | **Humble** | [`humble`](https://github.com/mateusmenezes95/omnidirectional_controllers/tree/humble) | [![Build From Source](https://github.com/mateusmenezes95/omnidirectional_controllers/actions/workflows/humble-source-build.yaml/badge.svg)](https://github.com/mateusmenezes95/omnidirectional_controllers/actions/workflows/humble-source-build.yaml) 15 | 16 | ## Installation Premises 17 | 18 | 1. This repository has been tested on [ROS2 Humble] and with [Classic Gazebo 11]; 19 | 20 | 2. These instructions assume that you have already installed ROS2 Humble Hawskbill on your machine. If not, please follow the recommended [recommended ubuntu installation tutorial]; 21 | 22 | 3. Before installing the package, you will need to have an ament workspace set up. If you don't have one, follow the instructions in the [Creating a workspace tutorial]. Once you have created the workspace, clone this repository in the source folder of your workspace. 23 | 24 | ## Installation 25 | 26 | > **ATTENTION:** These commands assume that you have created a workspace called "ros_ws" in your home folder. If you used a different directory or name, please adjust the commands accordingly. 27 | 28 | After installing ROS2 and creating the workspace, clone this repository in your workspace: 29 | 30 | ``` 31 | cd ~/ros_ws/src 32 | git clone https://github.com/mateusmenezes95/omnidirectional_controllers 33 | ``` 34 | 35 | Install the binary dependencies by running the following command in the root of your workspace: 36 | 37 | ``` 38 | cd ~/ros_ws 39 | rosdep init 40 | rosdep update 41 | sudo apt update 42 | rosdep install --from-paths src/omnidirectional_controllers --ignore-src -r -y --rosdistro humble 43 | ``` 44 | 45 | If all dependencies are already installed, you should see the message "All required rosdeps installed successfully." 46 | 47 | ## Building 48 | 49 | Run the following command to build the package: 50 | 51 | ``` 52 | cd ~/ros_ws 53 | colcon build --symlink-install --event-handlers console_direct+ 54 | ``` 55 | 56 | > Run `colcon build --help` to understand the arguments passed! 57 | > If you want to generate the compile_commands.json file, add the argument `--cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=1` to the command above. 58 | 59 | After building the package, open a new terminal and navigate to your workspace. Then, source the overlay by running the following command: 60 | 61 | ``` 62 | source /opt/ros/foxy/setup.bash 63 | cd ~/ros_ws 64 | . install/local_setup.bash 65 | ``` 66 | 67 | > See [Source the overlay] to learn about underlay and overlay concepts. 68 | 69 | ## Usage 70 | 71 | You must follow the three steps explained in [Running the Framework for Your Robot](https://control.ros.org/master/doc/getting_started/getting_started.html#running-the-framework-for-your-robot) tutorial. 72 | 73 | For an concrete example of how to use the Omnidirectional controllers, refer to the Axebot simulation's [controller configuration], [ros2_control URDF], and [launch file]. 74 | 75 | ### Subscribed Topic 76 | 77 | * **`/omnidirectional_controller/cmd_vel_unstamped`** ([geometry_msgs/msg/Twist]) 78 | 79 | Velocity twist from which the controller extracts the x and y component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. 80 | 81 | ### Published Topic 82 | 83 | * **`/omnidirectional_controller/odom`** ([nav_msgs/msg/Odometry]) 84 | 85 | Robot odometry. The odometry can be computed from hardware feedback or using an open-loop approach, in which the integration is performed using the Twist command. You can select the approach in the configuration file. Additionally, you can choose either the Runge-Kutta or Euler Forward integration method. 86 | 87 | ## Unit Test 88 | 89 | > TODO 90 | 91 | ## Bugs & Feature Requests 92 | 93 | Please report bugs and request features using the [Issue Tracker] 94 | 95 | [ros2_control]: https://control.ros.org/master/index.html 96 | [ros2_controllers]: https://control.ros.org/master/doc/ros2_controllers/doc/controllers_index.html 97 | [Issue Tracker]: https://github.com/mateusmenezes95/omnidirectional_controllers/issues 98 | [diff_drive_controller]: https://control.ros.org/master/doc/ros2_controllers/diff_drive_controller/doc/userdoc.html 99 | [ros2_control URDF]: https://github.com/mateusmenezes95/axebot/blob/foxy/axebot_description/urdf/ros2_control.urdf.xacro 100 | [controller configuration]: https://github.com/mateusmenezes95/axebot/blob/foxy/axebot_control/config/omnidirectional_controller.yaml 101 | [launch file]: https://github.com/mateusmenezes95/axebot/blob/foxy/axebot_gazebo/launch/axebot.launch.py 102 | [Creating a workspace tutorial]: https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-A-Workspace/Creating-A-Workspace.html 103 | [recommended ubuntu installation tutorial]: https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html 104 | [Source the overlay]: https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-A-Workspace/Creating-A-Workspace.html#source-the-overlay 105 | [geometry_msgs/msg/Twist]: https://docs.ros2.org/latest/api/geometry_msgs/msg/Twist.html 106 | [nav_msgs/msg/Odometry]: https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html 107 | [Classic Gazebo 11]: https://classic.gazebosim.org/ 108 | -------------------------------------------------------------------------------- /doc/images/.gitattributes: -------------------------------------------------------------------------------- 1 | *.jpg filter=lfs diff=lfs merge=lfs -text 2 | -------------------------------------------------------------------------------- /doc/images/axebot-kinematic-frames.jpg: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:e166a3b03416a7a2c39e3210a8e247b32514967e87181efe4b02c9be13c6b29a 3 | size 18973 4 | -------------------------------------------------------------------------------- /doc/kinematics_and_odometry.md: -------------------------------------------------------------------------------- 1 | # Omnidirectional Robot Kinematics and Odometry 2 | 3 | - [Kinematics](#kinematics) 4 | - [Inverse](#inverse) 5 | - [Forward](#forward) 6 | - [Odometry](#odometry) 7 | - [References](#references) 8 | 9 | ## Kinematics 10 | 11 | The figure below describe the geometry paramaters used in the forward and inverse kinematics implemented on this controller. 12 | 13 |

14 | 15 |
16 | Source: [1] 17 |

18 | 19 | ### Inverse 20 | 21 | From the geometry analysis in the above figure, we have that: 22 | 23 | ```math 24 | \begin{align} 25 | v_{m_1} &= -v_n + \omega L\\ 26 | v_{m_2} &=v\cos(\gamma) + v_n\sin(\gamma) + \omega L\\ 27 | v_{m_3} &=-v\cos(\gamma) + v_n\sin(\gamma) + \omega L 28 | \end{align} 29 | ``` 30 | 31 | Or in Matrix form: 32 | 33 | ```math 34 | \begin{equation} 35 | \begin{bmatrix} 36 | v_{m_1}\\ 37 | v_{m_2}\\ 38 | v_{m_3} 39 | \end{bmatrix}= 40 | \begin{bmatrix} 41 | 0 & -1 & L\\ 42 | \cos(\gamma) & \sin(\gamma) & L\\ 43 | -\cos(\gamma) & \sin(\gamma) & L\\ 44 | \end{bmatrix} 45 | \begin{bmatrix} 46 | v\\ 47 | v_{n}\\ 48 | \omega 49 | \end{bmatrix} 50 | \end{equation} 51 | ``` 52 | 53 | For the wheels’ angular velocities, assuming that all wheels have the same radius $r$ 54 | 55 | ```math 56 | \begin{equation} 57 | \begin{bmatrix} 58 | \omega_{m_1}\\ 59 | \omega_{m_2}\\ 60 | \omega_{m_3} 61 | \end{bmatrix} 62 | = 63 | \frac{1}{r} 64 | \begin{bmatrix} 65 | 0 & -1 & L\\ 66 | \cos(\gamma) & \sin(\gamma) & L\\ 67 | -\cos(\gamma) & \sin(\gamma) & L\\ 68 | \end{bmatrix} 69 | \begin{bmatrix} 70 | v\\ 71 | v_{n}\\ 72 | \omega 73 | \end{bmatrix} 74 | \end{equation} 75 | ``` 76 | 77 | ### Forward 78 | 79 | Manipulating the inverse kinematics, we get in 80 | 81 | ```math 82 | \begin{equation} 83 | \begin{bmatrix} 84 | v\\ 85 | v_{n}\\ 86 | \omega 87 | \end{bmatrix}=r 88 | \begin{bmatrix} 89 | 0&\frac{1}{2\cos(\gamma)}&\frac{-1}{2\cos(\gamma)}\\ 90 | \frac{-1}{\sin(\gamma)+1}&\frac{1}{2(\sin(\gamma)+1)}&\frac{1}{2(\sin(\gamma)+1)}\\ 91 | \frac{\sin(\gamma)}{L(\sin(\gamma)+1)}&\frac{1}{2L(\sin(\gamma)+1)}&\frac{1}{2L(\sin(\gamma)+1)} 92 | \end{bmatrix} 93 | \begin{bmatrix} 94 | \omega_{m_1}\\ 95 | \omega_{m_2}\\ 96 | \omega_{m_3} 97 | \end{bmatrix} 98 | \end{equation} 99 | ``` 100 | 101 | Or 102 | 103 | ```math 104 | \begin{equation} 105 | \begin{bmatrix} 106 | v\\ 107 | v_{n}\\ 108 | \omega 109 | \end{bmatrix} 110 | = 111 | rG^{-1} 112 | \begin{bmatrix} 113 | \omega_{m_1}\\ 114 | \omega_{m_2}\\ 115 | \omega_{m_3} 116 | \end{bmatrix} 117 | \end{equation} 118 | ``` 119 | 120 | Whereby 121 | 122 | ```math 123 | \begin{equation} 124 | G^{-1} = 125 | \begin{bmatrix} 126 | 0&\frac{1}{2\cos(\gamma)}&\frac{-1}{2\cos(\gamma)}\\ 127 | \frac{-1}{\sin(\gamma)+1}&\frac{1}{2(\sin(\gamma)+1)}&\frac{1}{2(\sin(\gamma)+1)}\\ 128 | \frac{\sin(\gamma)}{L(\sin(\gamma)+1)}&\frac{1}{2L(\sin(\gamma)+1)}&\frac{1}{2L(\sin(\gamma)+1)} 129 | \end{bmatrix} 130 | \end{equation} 131 | ``` 132 | 133 | Given that $\beta = \frac{1}{2\cos(\gamma)}$ and $\alpha = \frac{1}{\sin(\gamma)+1}$, we have 134 | 135 | ```math 136 | \begin{equation} 137 | G^{-1}= 138 | \begin{bmatrix} 139 | 0 & \beta & -\beta\\ 140 | -\alpha & 0.5\alpha & 0.5\alpha\\ 141 | L^{-1}\sin(\gamma)\alpha & 0.5L^{-1}\alpha & 0.5L^{-1}\alpha 142 | \end{bmatrix} 143 | \end{equation} 144 | ``` 145 | 146 | Therefore 147 | 148 | ```math 149 | \begin{align} 150 | v &= \beta(\omega_{m_2} - \omega_{m_3})\\ 151 | v_n &=\alpha(-w_{m_1} +0.5(w_{m_2}+w_{m_3}))\\ 152 | \omega &= L^{-1}\alpha(\sin(\gamma)w_{m_1}+0.5(w_{m_2}+w_{m_3})) 153 | \end{align} 154 | ``` 155 | 156 | ## Odometry 157 | 158 | The robot’s velocity with regard to the world (w.r.t) is given by 159 | 160 | ```math 161 | \begin{equation} 162 | \begin{bmatrix} 163 | \dot{x}_r\\ 164 | \dot{y}_r\\ 165 | \dot{\theta} 166 | \end{bmatrix} 167 | = 168 | R(\theta) 169 | \begin{bmatrix} 170 | v\\ 171 | v_{n}\\ 172 | \omega 173 | \end{bmatrix} 174 | \end{equation} 175 | ``` 176 | 177 | where $R(\theta)$ is the rotation matrix around the $z$ axis perpendicular to the horizontal plane $xy$, given by 178 | 179 | ```math 180 | \begin{equation} 181 | R(\theta) 182 | = 183 | \begin{bmatrix} 184 | \cos(\theta) & -\sin(\theta) & 0\\ 185 | \sin(\theta) & \cos(\theta) & 0\\ 186 | 0 & 0 & 1 187 | \end{bmatrix} 188 | \end{equation} 189 | ``` 190 | 191 | Assume that the robot configuration $q(t_k) = q_k = (x_k,y_k,\theta_k)$ at the sampling time $t_k$ is known, together with the velocity inputs $v_k$, $v_{n_k}$, and $w_k$ applied in the interval $[t_k,\:t_{k+1})$. The value of the configuration variable $q_{k+1}$ at the sampling time $t_{k+1}$ can then be reconstructed by forward integration of the kinematic model. Adapted from [2]. 192 | 193 | Considering that the sampling period $T_s = t_{k+1} - t_k$, the Euler method for the axebot configuration $q_{k+1}$ is given by 194 | 195 | ```math 196 | \begin{align} 197 | x_{k+1} &= x_k + (v\cos(\theta_k)-v_n\sin(\theta_k))T_s\\ 198 | y_{k+1} &= y_k + (v\sin(\theta_k)+v_n\cos(\theta_k))T_s\\ 199 | \theta_{k+1} &= \theta_k + \omega_kT_s 200 | \end{align} 201 | ``` 202 | 203 | And for the second-order Runge Kutta integration method 204 | 205 | ```math 206 | \begin{align} 207 | \bar{\theta}_k &= \theta_k+\frac{w_kTs}{2}\\ 208 | x_{k+1} &= x_k + (v\cos(\bar{\theta}_k)-v_n\sin(\bar{\theta}_k))T_s\\ 209 | y_{k+1} &= y_k + (v\sin(\bar{\theta}_k)+v_n\cos(\bar{\theta}_k))T_s\\ 210 | \theta_{k+1} &= \theta_k + \omega T_s 211 | \end{align} 212 | ``` 213 | 214 | ## References 215 | 216 | [1] J. Santos, A. Conceição, T. Santos, and H. Ara ujo, “Remote control of an omnidirectional mobile robot with time-varying delay and noise attenuation,” *Mechatronics*, vol. 52, pp. 7–21, 2018, ISSN: 0957-4158. DOI: 10.1016/j.mechatronics.2018.04.003. [Online](https://www.sciencedirect.com/science/article/abs/pii/S0957415818300606). 217 | 218 | [2] B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo, *Robotics: Modelling, Planning and Control*, 1st ed. Springer Publishing Company, Incorporated, 2008. 219 | -------------------------------------------------------------------------------- /include/omnidirectional_controllers/kinematics.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2022 Mateus Menezes 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #ifndef OMNIDIRECTIONAL_CONTROLLERS__KINEMATICS_HPP_ 24 | #define OMNIDIRECTIONAL_CONTROLLERS__KINEMATICS_HPP_ 25 | 26 | #include 27 | #include 28 | 29 | #include "omnidirectional_controllers/types.hpp" 30 | 31 | namespace omnidirectional_controllers { 32 | 33 | constexpr double OMNI_ROBOT_MAX_WHEELS = 4; 34 | 35 | class Kinematics { 36 | public: 37 | explicit Kinematics(RobotParams robot_params); 38 | Kinematics(); 39 | ~Kinematics(); 40 | // Forward kinematics 41 | RobotVelocity getBodyVelocity(const std::vector & wheels_vel); 42 | // Inverse kinematics 43 | std::vector getWheelsAngularVelocities(RobotVelocity vel); 44 | void setRobotParams(RobotParams robot_params); 45 | private: 46 | void initializeParams(); 47 | RobotParams robot_params_; 48 | std::vector angular_vel_vec_; 49 | double cos_gamma_; 50 | double sin_gamma_; 51 | double alpha_; 52 | double beta_; 53 | }; 54 | 55 | } // namespace omnidirectional_controllers 56 | 57 | #endif // OMNIDIRECTIONAL_CONTROLLERS__KINEMATICS_HPP_ 58 | -------------------------------------------------------------------------------- /include/omnidirectional_controllers/odometry.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2022 Mateus Menezes 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #ifndef OMNIDIRECTIONAL_CONTROLLERS__ODOMETRY_HPP_ 24 | #define OMNIDIRECTIONAL_CONTROLLERS__ODOMETRY_HPP_ 25 | 26 | #include 27 | #include 28 | 29 | #include "omnidirectional_controllers/kinematics.hpp" 30 | #include "omnidirectional_controllers/types.hpp" 31 | 32 | namespace omnidirectional_controllers { 33 | 34 | constexpr char EULER_FORWARD[] = "euler_forward"; 35 | constexpr char RUNGE_KUTTA2[] = "runge_kutta2"; 36 | 37 | class Odometry { 38 | public: 39 | Odometry(); 40 | ~Odometry(); 41 | bool setNumericIntegrationMethod(const std::string & numeric_integration_method); 42 | void setRobotParams(RobotParams params); 43 | void updateOpenLoop(RobotVelocity vel, double dt); 44 | void update(const std::vector & wheels_vel, double dt); 45 | RobotPose getPose() const; 46 | RobotVelocity getBodyVelocity() const; 47 | void reset(); 48 | 49 | protected: 50 | void integrateByRungeKutta(); 51 | void integrateByEuler(); 52 | void integrateVelocities(); 53 | RobotVelocity body_vel_{0, 0, 0}; 54 | double dt_; 55 | 56 | private: 57 | RobotPose pose_{0, 0, 0}; 58 | RobotParams robot_params_{0, 0, 0}; 59 | std::string numeric_integration_method_ = EULER_FORWARD; 60 | Kinematics robot_kinematics_; 61 | bool is_robot_param_set_{false}; 62 | }; 63 | 64 | } // namespace omnidirectional_controllers 65 | 66 | #endif // OMNIDIRECTIONAL_CONTROLLERS__ODOMETRY_HPP_ 67 | -------------------------------------------------------------------------------- /include/omnidirectional_controllers/omnidirectional_controller.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2022 Mateus Menezes 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #ifndef OMNIDIRECTIONAL_CONTROLLERS__OMNIDIRECTIONAL_CONTROLLER_HPP_ 24 | #define OMNIDIRECTIONAL_CONTROLLERS__OMNIDIRECTIONAL_CONTROLLER_HPP_ 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include "controller_interface/controller_interface.hpp" 31 | #include "geometry_msgs/msg/twist.hpp" 32 | #include "geometry_msgs/msg/twist_stamped.hpp" 33 | #include "nav_msgs/msg/odometry.hpp" 34 | #include "rclcpp/rclcpp.hpp" 35 | #include "rclcpp_lifecycle/state.hpp" 36 | #include "tf2_msgs/msg/tf_message.hpp" 37 | 38 | #include "omnidirectional_controllers/kinematics.hpp" 39 | #include "omnidirectional_controllers/odometry.hpp" 40 | #include "omnidirectional_controllers/types.hpp" 41 | 42 | namespace omnidirectional_controllers { 43 | using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; 44 | using StateInterfaceReferenceWrapper = std::reference_wrapper; 45 | using CommandInterfaceReferenceWrapper = std::reference_wrapper; 46 | 47 | class OmnidirectionalController : public controller_interface::ControllerInterface { 48 | public: 49 | OmnidirectionalController(); 50 | controller_interface::InterfaceConfiguration command_interface_configuration() const override; 51 | controller_interface::InterfaceConfiguration state_interface_configuration() const override; 52 | CallbackReturn on_init() override; 53 | CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; 54 | CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; 55 | CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; 56 | CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; 57 | CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; 58 | CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; 59 | controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; 60 | ~OmnidirectionalController(); 61 | 62 | protected: 63 | 64 | std::vector wheel_names_; 65 | std::vector registered_wheel_state_ifs_; 66 | std::vector registered_wheel_cmd_ifs_; 67 | 68 | // Default parameters for axebot 69 | RobotParams robot_params_{0.1, 0.0505, 0.0}; 70 | 71 | struct OdometryParams { 72 | bool open_loop = false; 73 | bool enable_odom_tf = true; 74 | std::string base_frame_id = "base_link"; 75 | std::string odom_frame_id = "odom"; 76 | std::string odom_numeric_integration_method = EULER_FORWARD; 77 | std::array pose_covariance_diagonal; 78 | std::array twist_covariance_diagonal; 79 | } odom_params_; 80 | 81 | bool use_stamped_vel_ = true; 82 | 83 | Kinematics omni_robot_kinematics_; 84 | Odometry odometry_; 85 | 86 | rclcpp::Publisher::SharedPtr odometry_publisher_ = nullptr; 87 | rclcpp::Publisher::SharedPtr odometry_transform_publisher_ = nullptr; 88 | 89 | tf2_msgs::msg::TFMessage odometry_transform_message_; 90 | nav_msgs::msg::Odometry odometry_message_; 91 | 92 | // Timeout to consider cmd_vel commands old 93 | std::chrono::milliseconds cmd_vel_timeout_{500}; 94 | 95 | bool subscriber_is_active_ = false; 96 | rclcpp::Subscription::SharedPtr vel_cmd_subscriber_ = nullptr; 97 | rclcpp::Subscription::SharedPtr 98 | vel_cmd_unstamped_subscriber_ = nullptr; 99 | 100 | rclcpp::Time previous_update_timestamp_{0}; 101 | 102 | double publish_rate_{50.0}; 103 | rclcpp::Duration publish_period_{0, 0}; 104 | rclcpp::Time previous_publish_timestamp_{0}; 105 | 106 | private: 107 | void velocityCommandStampedCallback(const geometry_msgs::msg::TwistStamped::SharedPtr cmd_vel); 108 | void velocityCommandUnstampedCallback(const geometry_msgs::msg::Twist::SharedPtr cmd_vel); 109 | geometry_msgs::msg::TwistStamped::SharedPtr cmd_vel_; 110 | double cos_gamma{0}; 111 | double sin_gamma{0}; 112 | 113 | std::shared_ptr node_; 114 | }; 115 | 116 | } // namespace omnidirectional_controllers 117 | 118 | #endif // OMNIDIRECTIONAL_CONTROLLERS__OMNIDIRECTIONAL_CONTROLLER_HPP_ 119 | -------------------------------------------------------------------------------- /include/omnidirectional_controllers/types.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2022 Mateus Menezes 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #ifndef OMNIDIRECTIONAL_CONTROLLERS__TYPES_HPP_ 24 | #define OMNIDIRECTIONAL_CONTROLLERS__TYPES_HPP_ 25 | 26 | #define DEG2RAD(deg) (deg * M_PI / 180.0) 27 | 28 | namespace omnidirectional_controllers { 29 | 30 | struct RobotParams { 31 | double wheel_radius; 32 | double robot_radius; 33 | double gamma; 34 | }; 35 | 36 | struct RobotVelocity { 37 | double vx; // [m/s] 38 | double vy; // [m/s] 39 | double omega; // [rad] 40 | }; 41 | 42 | struct RobotPose { 43 | double x; // [m] 44 | double y; // [m] 45 | double theta; // [rad] 46 | }; 47 | 48 | } // namespace omnidirectional_controllers 49 | 50 | #endif // OMNIDIRECTIONAL_CONTROLLERS__TYPES_HPP_ 51 | -------------------------------------------------------------------------------- /omnidirectional_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | The Omnidirectional controller transforms linear and angular velocity messages into signals for each wheel(s) for a Omnidirectional robot with three wheels. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | omnidirectional_controllers 5 | 0.0.0 6 | Controller for a omnidirectional robot with three wheels 7 | Mateus Menezes 8 | MIT 9 | 10 | ament_cmake 11 | 12 | controller_interface 13 | geometry_msgs 14 | hardware_interface 15 | nav_msgs 16 | pluginlib 17 | rclcpp_lifecycle 18 | rclcpp 19 | tf2_msgs 20 | tf2_ros 21 | tf2 22 | 23 | ament_cmake_gmock 24 | ament_lint_auto 25 | ament_lint_common 26 | controller_manager 27 | hardware_interface 28 | ros2_control_test_assets 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | -------------------------------------------------------------------------------- /src/kinematics.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2022 Mateus Menezes 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include 24 | #include 25 | 26 | #include "omnidirectional_controllers/kinematics.hpp" 27 | 28 | #include "omnidirectional_controllers/types.hpp" 29 | 30 | namespace omnidirectional_controllers { 31 | 32 | Kinematics::Kinematics(RobotParams robot_params) 33 | : robot_params_(robot_params) { 34 | this->initializeParams(); 35 | } 36 | 37 | Kinematics::Kinematics() { 38 | this->initializeParams(); 39 | } 40 | 41 | RobotVelocity Kinematics::getBodyVelocity(const std::vector & wheels_vel) { 42 | RobotVelocity vel; 43 | double wm1 = wheels_vel.at(0); 44 | double wm2 = wheels_vel.at(1); 45 | double wm3 = wheels_vel.at(2); 46 | 47 | vel.vx = beta_ * (wm2 - wm3); 48 | vel.vy = alpha_ * (-wm1 + (0.5 * (wm2 + wm3))); 49 | vel.omega = ((1/robot_params_.robot_radius)*alpha_) * ((sin_gamma_ * wm1) + (0.5 * (wm2 + wm3))); 50 | 51 | vel.vx *= robot_params_.wheel_radius; 52 | vel.vy *= robot_params_.wheel_radius; 53 | vel.omega *= robot_params_.wheel_radius; 54 | 55 | return vel; 56 | } 57 | 58 | std::vector Kinematics::getWheelsAngularVelocities(RobotVelocity vel) { 59 | double vx = vel.vx; 60 | double vy = vel.vy; 61 | double wl = vel.omega * robot_params_.robot_radius; 62 | 63 | angular_vel_vec_[0] = (-vy + wl) / robot_params_.wheel_radius; 64 | angular_vel_vec_[1] = ((vx * cos_gamma_) + (vy * sin_gamma_) + wl) / robot_params_.wheel_radius; 65 | angular_vel_vec_[2] = ((-vx * cos_gamma_) + (vy * sin_gamma_) + wl) / robot_params_.wheel_radius; 66 | 67 | return angular_vel_vec_; 68 | } 69 | 70 | void Kinematics::setRobotParams(RobotParams robot_params) { 71 | this->robot_params_ = robot_params; 72 | this->initializeParams(); 73 | } 74 | 75 | void Kinematics::initializeParams() { 76 | angular_vel_vec_.reserve(OMNI_ROBOT_MAX_WHEELS); 77 | angular_vel_vec_ = {0, 0, 0, 0}; 78 | cos_gamma_ = cos(robot_params_.gamma); 79 | sin_gamma_ = sin(robot_params_.gamma); 80 | alpha_ = 1 / (sin_gamma_ + 1); 81 | beta_ = 1 / (2*cos_gamma_); 82 | } 83 | 84 | Kinematics::~Kinematics() {} 85 | 86 | } // namespace omnidirectional_controllers 87 | -------------------------------------------------------------------------------- /src/odometry.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2022 Mateus Menezes 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include "omnidirectional_controllers/odometry.hpp" 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include "omnidirectional_controllers/types.hpp" 32 | 33 | namespace omnidirectional_controllers { 34 | 35 | Odometry::Odometry() {} 36 | 37 | bool Odometry::setNumericIntegrationMethod(const std::string & numeric_integration_method) { 38 | if (numeric_integration_method != EULER_FORWARD && 39 | numeric_integration_method != RUNGE_KUTTA2) { 40 | this->numeric_integration_method_ = EULER_FORWARD; 41 | return false; 42 | } 43 | this->numeric_integration_method_ = numeric_integration_method; 44 | return true; 45 | } 46 | 47 | void Odometry::setRobotParams(RobotParams params) { 48 | if (params.wheel_radius < std::numeric_limits::epsilon()) { 49 | std::stringstream error; 50 | error << "Invalid wheel radius " << params.wheel_radius << " set!" << std::endl; 51 | throw std::runtime_error(error.str()); 52 | } 53 | 54 | if (params.robot_radius < std::numeric_limits::epsilon()) { 55 | std::stringstream error; 56 | error << "Invalid robot radius " << params.wheel_radius << " set!" << std::endl; 57 | throw std::runtime_error(error.str()); 58 | } 59 | 60 | this->robot_params_ = params; 61 | robot_kinematics_.setRobotParams(robot_params_); 62 | is_robot_param_set_ = true; 63 | } 64 | 65 | void Odometry::updateOpenLoop(RobotVelocity vel, double dt_) { 66 | this->body_vel_ = vel; 67 | this->dt_ = dt_; 68 | this->integrateVelocities(); 69 | } 70 | 71 | void Odometry::update(const std::vector & wheels_vel, double dt) { 72 | if (!is_robot_param_set_) { 73 | throw std::runtime_error(std::string("Robot parameters was not set or not set properly!")); 74 | } 75 | this->dt_ = dt; 76 | this->body_vel_ = robot_kinematics_.getBodyVelocity(wheels_vel); 77 | this->integrateVelocities(); 78 | } 79 | 80 | RobotPose Odometry::getPose() const { 81 | return pose_; 82 | } 83 | 84 | RobotVelocity Odometry::getBodyVelocity() const { 85 | return body_vel_; 86 | } 87 | 88 | void Odometry::reset() { 89 | pose_ = {0, 0, 0}; 90 | } 91 | 92 | void Odometry::integrateByRungeKutta() { 93 | double theta_bar = pose_.theta + (body_vel_.omega*dt_ / 2); 94 | pose_.x = pose_.x + (body_vel_.vx * cos(theta_bar) - body_vel_.vy * sin(theta_bar)) * dt_; 95 | pose_.y = pose_.y + (body_vel_.vx * sin(theta_bar) + body_vel_.vy * cos(theta_bar)) * dt_; 96 | pose_.theta = pose_.theta + body_vel_.omega*dt_; 97 | } 98 | 99 | void Odometry::integrateByEuler() { 100 | pose_.x = pose_.x + (body_vel_.vx * cos(pose_.theta) - body_vel_.vy * sin(pose_.theta)) * dt_; 101 | pose_.y = pose_.y + (body_vel_.vx * sin(pose_.theta) + body_vel_.vy * cos(pose_.theta)) * dt_; 102 | pose_.theta = pose_.theta + body_vel_.omega*dt_; 103 | } 104 | 105 | void Odometry::integrateVelocities() { 106 | if (numeric_integration_method_ == RUNGE_KUTTA2) { 107 | this->integrateByRungeKutta(); 108 | return; 109 | } 110 | // Euler method is the odometry class default! 111 | this->integrateByEuler(); 112 | this->dt_ = 0; 113 | this->body_vel_ = {0, 0, 0}; 114 | } 115 | 116 | Odometry::~Odometry() {} 117 | 118 | } // namespace omnidirectional_controllers 119 | -------------------------------------------------------------------------------- /src/omnidirectional_controller.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2022 Mateus Menezes 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include "omnidirectional_controllers/omnidirectional_controller.hpp" 24 | 25 | #include // NOLINT 26 | #include 27 | #include 28 | 29 | #include "geometry_msgs/msg/twist.hpp" 30 | #include "geometry_msgs/msg/twist_stamped.hpp" 31 | #include "hardware_interface/types/hardware_interface_type_values.hpp" 32 | #include "lifecycle_msgs/msg/state.hpp" 33 | #include "rclcpp/logging.hpp" 34 | #include "tf2/LinearMath/Quaternion.h" 35 | 36 | namespace { 37 | constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel"; 38 | constexpr auto DEFAULT_COMMAND_UNSTAMPED_TOPIC = "~/cmd_vel_unstamped"; 39 | constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "~/cmd_vel_out"; 40 | constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom"; 41 | constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; 42 | constexpr auto WHEELS_QUANTITY = 3; 43 | } // namespace 44 | 45 | namespace omnidirectional_controllers { 46 | 47 | using namespace std::chrono_literals; // NOLINT 48 | using controller_interface::interface_configuration_type; 49 | using controller_interface::InterfaceConfiguration; 50 | using hardware_interface::HW_IF_POSITION; 51 | using hardware_interface::HW_IF_VELOCITY; 52 | using lifecycle_msgs::msg::State; 53 | using std::placeholders::_1; 54 | 55 | OmnidirectionalController::OmnidirectionalController() 56 | : controller_interface::ControllerInterface() 57 | , cmd_vel_(std::make_shared()) {} 58 | 59 | CallbackReturn OmnidirectionalController::on_init() { 60 | this->node_ = this->get_node(); 61 | 62 | try { 63 | auto_declare>("wheel_names", std::vector()); 64 | 65 | auto_declare("robot_radius", robot_params_.robot_radius); 66 | auto_declare("wheel_radius", robot_params_.wheel_radius); 67 | auto_declare("gamma", robot_params_.gamma); 68 | 69 | auto_declare("odom_frame_id", odom_params_.odom_frame_id); 70 | auto_declare("base_frame_id", odom_params_.base_frame_id); 71 | auto_declare("odom_numeric_integration_method", 72 | odom_params_.odom_numeric_integration_method); 73 | auto_declare>("pose_covariance_diagonal", std::vector()); 74 | auto_declare>("twist_covariance_diagonal", std::vector()); 75 | auto_declare("open_loop", odom_params_.open_loop); 76 | auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); 77 | 78 | auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0); 79 | auto_declare("velocity_rolling_window_size", 10); 80 | auto_declare("use_stamped_vel", use_stamped_vel_); 81 | } catch (const std::exception & e) { 82 | fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); 83 | return CallbackReturn::ERROR; 84 | } 85 | 86 | return CallbackReturn::SUCCESS; 87 | } 88 | 89 | InterfaceConfiguration OmnidirectionalController::command_interface_configuration() const { 90 | std::vector conf_names; 91 | 92 | for (const auto & joint_name : wheel_names_) { 93 | conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); 94 | } 95 | 96 | return {interface_configuration_type::INDIVIDUAL, conf_names}; 97 | } 98 | 99 | InterfaceConfiguration OmnidirectionalController::state_interface_configuration() const { 100 | if (odom_params_.open_loop) { //return an empty configuration if "open loop" mode is activated. 101 | return {interface_configuration_type::NONE, {}}; 102 | } 103 | std::vector conf_names; 104 | for (const auto & joint_name : wheel_names_) { 105 | conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); 106 | } 107 | return {interface_configuration_type::INDIVIDUAL, conf_names}; 108 | } 109 | 110 | CallbackReturn OmnidirectionalController::on_configure( 111 | const rclcpp_lifecycle::State & previous_state) { 112 | auto logger = node_->get_logger(); 113 | 114 | RCLCPP_DEBUG(logger, 115 | "Called on_configure. Previous state was %s", 116 | previous_state.label().c_str()); 117 | 118 | // update parameters 119 | wheel_names_ = node_->get_parameter("wheel_names").as_string_array(); 120 | 121 | if (wheel_names_.size() != WHEELS_QUANTITY) { 122 | RCLCPP_ERROR( 123 | logger, "The number of wheels [%zu] and the required [%d] are different", 124 | wheel_names_.size(), WHEELS_QUANTITY); 125 | return CallbackReturn::ERROR; 126 | } 127 | 128 | if (wheel_names_.empty()) { 129 | RCLCPP_ERROR(logger, "Wheel names parameters are empty!"); 130 | return CallbackReturn::ERROR; 131 | } 132 | 133 | robot_params_.robot_radius = node_->get_parameter("robot_radius").as_double(); 134 | robot_params_.wheel_radius = node_->get_parameter("wheel_radius").as_double(); 135 | robot_params_.gamma = node_->get_parameter("gamma").as_double(); 136 | robot_params_.gamma = DEG2RAD(robot_params_.gamma); 137 | 138 | omni_robot_kinematics_.setRobotParams(robot_params_); 139 | odometry_.setRobotParams(robot_params_); 140 | 141 | odom_params_.odom_frame_id = node_->get_parameter("odom_frame_id").as_string(); 142 | odom_params_.base_frame_id = node_->get_parameter("base_frame_id").as_string(); 143 | 144 | auto pose_diagonal = node_->get_parameter("pose_covariance_diagonal").as_double_array(); 145 | std::copy( 146 | pose_diagonal.begin(), pose_diagonal.end(), odom_params_.pose_covariance_diagonal.begin()); 147 | 148 | auto twist_diagonal = node_->get_parameter("twist_covariance_diagonal").as_double_array(); 149 | std::copy( 150 | twist_diagonal.begin(), twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin()); 151 | 152 | odom_params_.open_loop = node_->get_parameter("open_loop").as_bool(); 153 | odom_params_.enable_odom_tf = node_->get_parameter("enable_odom_tf").as_bool(); 154 | odom_params_.odom_numeric_integration_method = node_->get_parameter( 155 | "odom_numeric_integration_method").as_string(); 156 | 157 | if (odom_params_.odom_numeric_integration_method != EULER_FORWARD && 158 | odom_params_.odom_numeric_integration_method != RUNGE_KUTTA2) { 159 | RCLCPP_WARN(logger, 160 | "Invalid numeric integration got: %s. Using default %s intead", 161 | odom_params_.odom_numeric_integration_method.c_str(), 162 | EULER_FORWARD); 163 | odom_params_.odom_numeric_integration_method = EULER_FORWARD; 164 | } 165 | odometry_.setNumericIntegrationMethod(odom_params_.odom_numeric_integration_method); 166 | 167 | cmd_vel_timeout_ = std::chrono::milliseconds{ 168 | static_cast(node_->get_parameter("cmd_vel_timeout").as_double() * 1000.0)}; 169 | use_stamped_vel_ = node_->get_parameter("use_stamped_vel").as_bool(); 170 | 171 | // initialize command subscriber 172 | if (use_stamped_vel_) { 173 | vel_cmd_subscriber_ = node_->create_subscription( 174 | DEFAULT_COMMAND_TOPIC, 175 | rclcpp::SystemDefaultsQoS(), 176 | std::bind(&OmnidirectionalController::velocityCommandStampedCallback, this, _1)); 177 | } else { 178 | vel_cmd_unstamped_subscriber_ = node_->create_subscription( 179 | DEFAULT_COMMAND_UNSTAMPED_TOPIC, 180 | rclcpp::SystemDefaultsQoS(), 181 | std::bind(&OmnidirectionalController::velocityCommandUnstampedCallback, this, _1)); 182 | } 183 | 184 | // initialize odometry publisher and messasge 185 | odometry_publisher_ = node_->create_publisher( 186 | DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); 187 | 188 | // limit the publication on the topics /odom and /tf 189 | publish_rate_ = node_->get_parameter("publish_rate").as_double(); 190 | publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); 191 | previous_publish_timestamp_ = node_->get_clock()->now(); 192 | 193 | // initialize odom values zeros 194 | odometry_message_.twist = 195 | geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); 196 | 197 | constexpr size_t NUM_DIMENSIONS = 6; 198 | for (size_t index = 0; index < 6; ++index) { 199 | // 0, 7, 14, 21, 28, 35 200 | const size_t diagonal_index = NUM_DIMENSIONS * index + index; 201 | odometry_message_.pose.covariance[diagonal_index] = 202 | odom_params_.pose_covariance_diagonal[index]; 203 | odometry_message_.twist.covariance[diagonal_index] = 204 | odom_params_.twist_covariance_diagonal[index]; 205 | } 206 | 207 | // initialize transform publisher and message 208 | odometry_transform_publisher_ = node_->create_publisher( 209 | DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); 210 | 211 | // keeping track of odom and base_link transforms only 212 | odometry_transform_message_.transforms.resize(1); 213 | odometry_transform_message_.transforms.front().header.frame_id = odom_params_.odom_frame_id; 214 | odometry_transform_message_.transforms.front().child_frame_id = odom_params_.base_frame_id; 215 | 216 | return CallbackReturn::SUCCESS; 217 | } 218 | 219 | CallbackReturn OmnidirectionalController::on_activate(const rclcpp_lifecycle::State & previous_state) { 220 | auto logger = node_->get_logger(); 221 | 222 | RCLCPP_DEBUG(logger, "Called on_activate. Previous state was %s", previous_state.label().c_str()); 223 | 224 | if (wheel_names_.empty()) { 225 | RCLCPP_ERROR(logger, "No wheel names specified"); 226 | return CallbackReturn::ERROR; 227 | } 228 | 229 | // register handles 230 | registered_wheel_state_ifs_.reserve(wheel_names_.size()); 231 | registered_wheel_cmd_ifs_.reserve(wheel_names_.size()); 232 | for (const auto & wheel_name : wheel_names_) { 233 | 234 | if (!odom_params_.open_loop) { 235 | std::string interface_name = wheel_name + "/" + HW_IF_VELOCITY; 236 | 237 | const auto state_handle = std::find_if(state_interfaces_.cbegin(), state_interfaces_.cend(), 238 | [&interface_name](const auto & interface) { 239 | return interface.get_name() == interface_name; 240 | } 241 | ); 242 | 243 | if (state_handle == state_interfaces_.cend()) { 244 | RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str()); 245 | return CallbackReturn::ERROR; 246 | } 247 | 248 | registered_wheel_state_ifs_.emplace_back(*state_handle); 249 | 250 | RCLCPP_INFO(logger, "Got state interface: %s", state_handle->get_name().c_str()); 251 | } 252 | 253 | const auto command_handle = std::find_if( 254 | command_interfaces_.begin(), command_interfaces_.end(), 255 | [&wheel_name](const auto & interface) { 256 | std::string interface_name = wheel_name + "/" + HW_IF_VELOCITY; 257 | return interface.get_name() == interface_name; 258 | } 259 | ); 260 | 261 | if (command_handle == command_interfaces_.end()) { 262 | RCLCPP_ERROR(logger, "Unable to obtain joint command handle for %s", wheel_name.c_str()); 263 | return CallbackReturn::ERROR; 264 | } 265 | 266 | registered_wheel_cmd_ifs_.emplace_back(*command_handle); 267 | 268 | RCLCPP_INFO(logger, "Got command interface: %s", command_handle->get_name().c_str()); 269 | } 270 | 271 | subscriber_is_active_ = true; 272 | 273 | RCLCPP_DEBUG(node_->get_logger(), "Subscriber and publisher are now active."); 274 | previous_update_timestamp_ = node_->get_clock()->now(); 275 | 276 | return CallbackReturn::SUCCESS; 277 | } 278 | 279 | CallbackReturn OmnidirectionalController::on_deactivate( 280 | const rclcpp_lifecycle::State & previous_state) { 281 | RCLCPP_DEBUG(node_->get_logger(), 282 | "Called on_deactivate. Previous state was %s", 283 | previous_state.label().c_str()); 284 | subscriber_is_active_ = false; 285 | odometry_.reset(); 286 | return CallbackReturn::SUCCESS; 287 | } 288 | 289 | CallbackReturn OmnidirectionalController::on_cleanup( 290 | const rclcpp_lifecycle::State & previous_state) { 291 | RCLCPP_DEBUG(node_->get_logger(), 292 | "Called on_cleanup. Previous state was %s", 293 | previous_state.label().c_str()); 294 | return CallbackReturn::SUCCESS; 295 | } 296 | 297 | CallbackReturn OmnidirectionalController::on_error( 298 | const rclcpp_lifecycle::State & previous_state) { 299 | RCLCPP_DEBUG(node_->get_logger(), 300 | "Called on_error. Previous state was %s", 301 | previous_state.label().c_str()); 302 | return CallbackReturn::SUCCESS; 303 | } 304 | 305 | CallbackReturn OmnidirectionalController::on_shutdown( 306 | const rclcpp_lifecycle::State & previous_state) { 307 | RCLCPP_DEBUG(node_->get_logger(), 308 | "Called on_error. Previous state was %s", 309 | previous_state.label().c_str()); 310 | return CallbackReturn::SUCCESS; 311 | } 312 | 313 | void OmnidirectionalController::velocityCommandStampedCallback( 314 | const geometry_msgs::msg::TwistStamped::SharedPtr cmd_vel) { 315 | if (!subscriber_is_active_) { 316 | RCLCPP_WARN(node_->get_logger(), "Can't accept new commands. subscriber is inactive"); 317 | return; 318 | } 319 | if ((cmd_vel->header.stamp.sec == 0) && (cmd_vel->header.stamp.nanosec == 0)) { 320 | RCLCPP_WARN_ONCE( 321 | node_->get_logger(), 322 | "Received TwistStamped with zero timestamp, setting it to current " 323 | "time, this message will only be shown once"); 324 | cmd_vel->header.stamp = node_->get_clock()->now(); 325 | } 326 | 327 | this->cmd_vel_ = std::move(cmd_vel); 328 | } 329 | 330 | void OmnidirectionalController::velocityCommandUnstampedCallback( 331 | const geometry_msgs::msg::Twist::SharedPtr cmd_vel) { 332 | if (!subscriber_is_active_) { 333 | RCLCPP_WARN(node_->get_logger(), "Can't accept new commands. subscriber is inactive"); 334 | return; 335 | } 336 | 337 | this->cmd_vel_->twist = *cmd_vel; 338 | this->cmd_vel_->header.stamp = node_->get_clock()->now(); 339 | } 340 | 341 | controller_interface::return_type OmnidirectionalController::update( 342 | const rclcpp::Time & time, 343 | const rclcpp::Duration & period) { 344 | auto logger = node_->get_logger(); 345 | const auto current_time = time; 346 | 347 | const auto dt = current_time - cmd_vel_->header.stamp; 348 | // Brake if cmd_vel has timeout, override the stored command 349 | if (dt > cmd_vel_timeout_) { 350 | cmd_vel_->twist.linear.x = 0.0; 351 | cmd_vel_->twist.linear.y = 0.0; 352 | cmd_vel_->twist.angular.z = 0.0; 353 | } 354 | 355 | if (odom_params_.open_loop) { 356 | odometry_.updateOpenLoop( 357 | {cmd_vel_->twist.linear.x, cmd_vel_->twist.linear.y, cmd_vel_->twist.angular.z}, 358 | period.seconds()); 359 | } else { 360 | std::vector wheels_angular_velocity({0, 0, 0}); 361 | wheels_angular_velocity[0] = registered_wheel_state_ifs_[0].get().get_value(); 362 | wheels_angular_velocity[1] = registered_wheel_state_ifs_[1].get().get_value(); 363 | wheels_angular_velocity[2] = registered_wheel_state_ifs_[2].get().get_value(); 364 | try { 365 | odometry_.update(wheels_angular_velocity, period.seconds()); 366 | } catch(const std::runtime_error& e) { 367 | RCLCPP_ERROR(logger, e.what()); 368 | rclcpp::shutdown(); 369 | } 370 | } 371 | 372 | tf2::Quaternion orientation; 373 | orientation.setRPY(0.0, 0.0, odometry_.getPose().theta); 374 | 375 | if (previous_publish_timestamp_ + publish_period_ < current_time) { 376 | previous_publish_timestamp_ += publish_period_; 377 | odometry_message_.header.stamp = current_time; 378 | odometry_message_.pose.pose.position.x = odometry_.getPose().x; 379 | odometry_message_.pose.pose.position.y = odometry_.getPose().y; 380 | odometry_message_.pose.pose.orientation.x = orientation.x(); 381 | odometry_message_.pose.pose.orientation.y = orientation.y(); 382 | odometry_message_.pose.pose.orientation.z = orientation.z(); 383 | odometry_message_.pose.pose.orientation.w = orientation.w(); 384 | odometry_message_.twist.twist.linear = cmd_vel_->twist.linear; 385 | odometry_message_.twist.twist.angular = cmd_vel_->twist.angular; 386 | odometry_publisher_->publish(odometry_message_); 387 | } 388 | 389 | // if (odom_params_.enable_odom_tf) { 390 | // odometry_transform_message_.header.stamp = current_time; 391 | // odometry_transform_message_.transform.translation.x = odometry_.getPose().x; 392 | // odometry_transform_message_.transform.translation.y = odometry_.getPose().y; 393 | // odometry_transform_message_.transform.rotation.x = orientation.x(); 394 | // odometry_transform_message_.transform.rotation.y = orientation.y(); 395 | // odometry_transform_message_.transform.rotation.z = orientation.z(); 396 | // odometry_transform_message_.transform.rotation.w = orientation.w(); 397 | // } 398 | 399 | // Compute wheels velocities: 400 | RobotVelocity body_vel_setpoint; 401 | body_vel_setpoint.vx = cmd_vel_->twist.linear.x; 402 | body_vel_setpoint.vy = cmd_vel_->twist.linear.y; 403 | body_vel_setpoint.omega = cmd_vel_->twist.angular.z; 404 | 405 | std::vector wheels_angular_velocity; 406 | wheels_angular_velocity = omni_robot_kinematics_.getWheelsAngularVelocities(body_vel_setpoint); 407 | 408 | // Set wheels velocities: 409 | registered_wheel_cmd_ifs_[0].get().set_value(wheels_angular_velocity.at(0)); 410 | registered_wheel_cmd_ifs_[1].get().set_value(wheels_angular_velocity.at(1)); 411 | registered_wheel_cmd_ifs_[2].get().set_value(wheels_angular_velocity.at(2)); 412 | 413 | 414 | return controller_interface::return_type::OK; 415 | } 416 | 417 | OmnidirectionalController::~OmnidirectionalController() {} 418 | 419 | } // namespace omnidirectional_controllers 420 | 421 | #include "class_loader/register_macro.hpp" 422 | 423 | CLASS_LOADER_REGISTER_CLASS( 424 | omnidirectional_controllers::OmnidirectionalController, controller_interface::ControllerInterface) 425 | -------------------------------------------------------------------------------- /test/test_kinematics.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2022 Mateus Menezes 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include 24 | 25 | #include 26 | 27 | #include "omnidirectional_controllers/kinematics.hpp" 28 | #include "omnidirectional_controllers/types.hpp" 29 | 30 | constexpr double TOLERANCE = 1e-3; 31 | 32 | class TestOmnidirectionalControllersKinematics : public ::testing::Test { 33 | protected: 34 | omnidirectional_controllers::Kinematics kinematics_; 35 | }; 36 | 37 | TEST_F(TestOmnidirectionalControllersKinematics, TestForwardKinematics) { 38 | std::vector angular_velocity; 39 | angular_velocity.reserve(3); 40 | kinematics_.setRobotParams({0.053112205, 0.1, DEG2RAD(30)}); 41 | 42 | omnidirectional_controllers::RobotVelocity vel; 43 | 44 | angular_velocity = {0.0, 1.63056, -1.63056}; 45 | vel = kinematics_.getBodyVelocity(angular_velocity); 46 | 47 | EXPECT_NEAR(vel.vx, 0.1, TOLERANCE); 48 | EXPECT_NEAR(vel.vy, 0, TOLERANCE); 49 | EXPECT_NEAR(vel.omega, 0, TOLERANCE); 50 | 51 | angular_velocity = {-1.88281, 0.94140, 0.94140}; 52 | vel = kinematics_.getBodyVelocity(angular_velocity); 53 | 54 | EXPECT_NEAR(vel.vx, 0, TOLERANCE); 55 | EXPECT_NEAR(vel.vy, 0.1, TOLERANCE); 56 | EXPECT_NEAR(vel.omega, 0, TOLERANCE); 57 | 58 | angular_velocity = {0.18828, 0.18828, 0.18828}; 59 | vel = kinematics_.getBodyVelocity(angular_velocity); 60 | 61 | EXPECT_NEAR(vel.vx, 0, TOLERANCE); 62 | EXPECT_NEAR(vel.vy, 0, TOLERANCE); 63 | EXPECT_NEAR(vel.omega, 0.1, TOLERANCE); 64 | 65 | angular_velocity = {-1.88281, 2.57196, -0.68915}; 66 | vel = kinematics_.getBodyVelocity(angular_velocity); 67 | 68 | EXPECT_NEAR(vel.vx, 0.1, TOLERANCE); 69 | EXPECT_NEAR(vel.vy, 0.1, TOLERANCE); 70 | EXPECT_NEAR(vel.omega, 0, TOLERANCE); 71 | } 72 | 73 | TEST_F(TestOmnidirectionalControllersKinematics, TestInverseKinematics) { 74 | std::vector angular_velocities; 75 | omnidirectional_controllers::RobotVelocity vel; 76 | kinematics_.setRobotParams({0.053112205, 0.1, DEG2RAD(30)}); 77 | 78 | vel = {0.1, 0, 0}; 79 | angular_velocities = kinematics_.getWheelsAngularVelocities(vel); 80 | 81 | ASSERT_GT(angular_velocities.size(), 0.0); 82 | EXPECT_NEAR(angular_velocities[0], 0.0, TOLERANCE); 83 | EXPECT_NEAR(angular_velocities[1], 1.63056, TOLERANCE); 84 | EXPECT_NEAR(angular_velocities[2], -1.63056, TOLERANCE); 85 | 86 | vel = {0, 0.1, 0}; 87 | angular_velocities = kinematics_.getWheelsAngularVelocities(vel); 88 | 89 | ASSERT_GT(angular_velocities.size(), 0.0); 90 | EXPECT_NEAR(angular_velocities[0], -1.88281, TOLERANCE); 91 | EXPECT_NEAR(angular_velocities[1], 0.94140, TOLERANCE); 92 | EXPECT_NEAR(angular_velocities[2], 0.94140, TOLERANCE); 93 | 94 | vel = {0, 0, 0.1}; 95 | angular_velocities = kinematics_.getWheelsAngularVelocities(vel); 96 | 97 | ASSERT_GT(angular_velocities.size(), 0.0); 98 | EXPECT_NEAR(angular_velocities[0], 0.18828, TOLERANCE); 99 | EXPECT_NEAR(angular_velocities[1], 0.18828, TOLERANCE); 100 | EXPECT_NEAR(angular_velocities[2], 0.18828, TOLERANCE); 101 | 102 | vel = {0.1, 0.1, 0}; 103 | angular_velocities = kinematics_.getWheelsAngularVelocities(vel); 104 | 105 | ASSERT_GT(angular_velocities.size(), 0.0); 106 | EXPECT_NEAR(angular_velocities[0], -1.88281, TOLERANCE); 107 | EXPECT_NEAR(angular_velocities[1], 2.57196, TOLERANCE); 108 | EXPECT_NEAR(angular_velocities[2], -0.68915, TOLERANCE); 109 | } 110 | -------------------------------------------------------------------------------- /test/test_odometry.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2022 Mateus Menezes 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include "omnidirectional_controllers/odometry.hpp" 30 | #include "omnidirectional_controllers/types.hpp" 31 | 32 | constexpr double TOLERANCE = 1e-6; 33 | 34 | class TestOmnidirectionalControllersOdometry : public ::testing::Test { 35 | protected: 36 | omnidirectional_controllers::Odometry odom_open_loop_; 37 | omnidirectional_controllers::Odometry odom_dead_reckoning_; 38 | 39 | void testMethodOnlyWithLinearVelocities(std::string method_name) { 40 | ASSERT_TRUE(odom_open_loop_.setNumericIntegrationMethod(method_name)); 41 | 42 | double time = 0; 43 | constexpr double dt = 0.1; 44 | 45 | omnidirectional_controllers::RobotVelocity vel = {1.0, 1.0, 0.0}; 46 | 47 | for (size_t i = 0; i < 10; i++) { 48 | time += dt; 49 | odom_open_loop_.updateOpenLoop(vel, dt); 50 | } 51 | 52 | EXPECT_NEAR(time, 1.0, TOLERANCE); 53 | EXPECT_NEAR(odom_open_loop_.getPose().x, 1.0, TOLERANCE); 54 | EXPECT_NEAR(odom_open_loop_.getPose().y, 1.0, TOLERANCE); 55 | EXPECT_NEAR(odom_open_loop_.getPose().theta, 0, TOLERANCE); 56 | 57 | odom_open_loop_.reset(); 58 | 59 | EXPECT_NEAR(odom_open_loop_.getPose().x, 0, TOLERANCE); 60 | EXPECT_NEAR(odom_open_loop_.getPose().y, 0, TOLERANCE); 61 | EXPECT_NEAR(odom_open_loop_.getPose().theta, 0, TOLERANCE); 62 | } 63 | }; 64 | 65 | TEST_F(TestOmnidirectionalControllersOdometry, TestIfNumericIntegrationMethodIsSetProperly) { 66 | ASSERT_TRUE(odom_open_loop_.setNumericIntegrationMethod("euler_forward")); 67 | ASSERT_TRUE(odom_open_loop_.setNumericIntegrationMethod("runge_kutta2")); 68 | ASSERT_FALSE(odom_open_loop_.setNumericIntegrationMethod("other_method")); 69 | } 70 | 71 | TEST_F(TestOmnidirectionalControllersOdometry, TestIfRobotParamSetThrowsExceptionWithInvalidArgs) { 72 | ASSERT_THROW(odom_open_loop_.setRobotParams({1.0, 0.0, 0.0}), std::runtime_error); 73 | ASSERT_THROW(odom_open_loop_.setRobotParams({0.0, 1.0, 1.0}), std::runtime_error); 74 | ASSERT_NO_THROW(odom_open_loop_.setRobotParams({1.0, 1.0, 0.0})); 75 | } 76 | 77 | TEST_F(TestOmnidirectionalControllersOdometry, 78 | TestEulerMethodOnlyWithLinearVelocitiesInOpenLoop) { 79 | this->testMethodOnlyWithLinearVelocities("euler_forward"); 80 | } 81 | 82 | TEST_F(TestOmnidirectionalControllersOdometry, 83 | TestEulerMethodWithLinearAndAngularVelocitiesInOpenLoop) { 84 | ASSERT_TRUE(odom_open_loop_.setNumericIntegrationMethod("euler_forward")); 85 | 86 | constexpr double dt = 0.1; 87 | omnidirectional_controllers::RobotVelocity vel = {1.0, 1.0, 1.0}; 88 | 89 | odom_open_loop_.updateOpenLoop(vel, dt); 90 | EXPECT_NEAR(odom_open_loop_.getPose().x, 0.1, TOLERANCE); 91 | EXPECT_NEAR(odom_open_loop_.getPose().y, 0.1, TOLERANCE); 92 | EXPECT_NEAR(odom_open_loop_.getPose().theta, 0.1, TOLERANCE); 93 | 94 | odom_open_loop_.updateOpenLoop(vel, dt); 95 | EXPECT_NEAR(odom_open_loop_.getPose().x, 0.209484, TOLERANCE); 96 | EXPECT_NEAR(odom_open_loop_.getPose().y, 0.189517, TOLERANCE); 97 | EXPECT_NEAR(odom_open_loop_.getPose().theta, 0.2, TOLERANCE); 98 | 99 | odom_open_loop_.updateOpenLoop(vel, dt); 100 | EXPECT_NEAR(odom_open_loop_.getPose().x, 0.327358, TOLERANCE); 101 | EXPECT_NEAR(odom_open_loop_.getPose().y, 0.267657, TOLERANCE); 102 | EXPECT_NEAR(odom_open_loop_.getPose().theta, 0.3, TOLERANCE); 103 | 104 | odom_open_loop_.reset(); 105 | 106 | EXPECT_NEAR(odom_open_loop_.getPose().x, 0, TOLERANCE); 107 | EXPECT_NEAR(odom_open_loop_.getPose().y, 0, TOLERANCE); 108 | EXPECT_NEAR(odom_open_loop_.getPose().theta, 0, TOLERANCE); 109 | } 110 | 111 | TEST_F(TestOmnidirectionalControllersOdometry, 112 | TestRungeKuttaMethodOnlyWithLinearVelocitiesInOpenLoop) { 113 | this->testMethodOnlyWithLinearVelocities("runge_kutta2"); 114 | } 115 | 116 | TEST_F(TestOmnidirectionalControllersOdometry, 117 | TestRungeKuttaMethodWithLinearAndAngularVelocitiesInOpenLoop) { 118 | ASSERT_TRUE(odom_open_loop_.setNumericIntegrationMethod("runge_kutta2")); 119 | 120 | constexpr double dt = 0.1; 121 | omnidirectional_controllers::RobotVelocity vel = {1.0, 1.0, 1.0}; 122 | 123 | odom_open_loop_.updateOpenLoop(vel, dt); 124 | EXPECT_NEAR(odom_open_loop_.getPose().x, 0.104873, TOLERANCE); 125 | EXPECT_NEAR(odom_open_loop_.getPose().y, 0.094877, TOLERANCE); 126 | EXPECT_NEAR(odom_open_loop_.getPose().theta, 0.1, TOLERANCE); 127 | 128 | odom_open_loop_.updateOpenLoop(vel, dt); 129 | EXPECT_NEAR(odom_open_loop_.getPose().x, 0.218694, TOLERANCE); 130 | EXPECT_NEAR(odom_open_loop_.getPose().y, 0.178810, TOLERANCE); 131 | EXPECT_NEAR(odom_open_loop_.getPose().theta, 0.2, TOLERANCE); 132 | 133 | odom_open_loop_.updateOpenLoop(vel, dt); 134 | EXPECT_NEAR(odom_open_loop_.getPose().x, 0.340326, TOLERANCE); 135 | EXPECT_NEAR(odom_open_loop_.getPose().y, 0.250961, TOLERANCE); 136 | EXPECT_NEAR(odom_open_loop_.getPose().theta, 0.3, TOLERANCE); 137 | 138 | odom_open_loop_.reset(); 139 | 140 | EXPECT_NEAR(odom_open_loop_.getPose().x, 0, TOLERANCE); 141 | EXPECT_NEAR(odom_open_loop_.getPose().y, 0, TOLERANCE); 142 | EXPECT_NEAR(odom_open_loop_.getPose().theta, 0, TOLERANCE); 143 | } 144 | 145 | TEST_F(TestOmnidirectionalControllersOdometry, 146 | TestRungeKuttaMethodWithLinearAndAngularVelocitiesInDeadReckoning) { 147 | ASSERT_TRUE(odom_dead_reckoning_.setNumericIntegrationMethod("runge_kutta2")); 148 | 149 | constexpr double dt = 0.1; 150 | std::vector wheels_vel{-16.94525768606293, 27.60242026826863, -5.008743353518062}; 151 | odom_dead_reckoning_.setRobotParams({0.053112205, 0.1, DEG2RAD(30)}); 152 | 153 | ASSERT_NO_THROW(odom_dead_reckoning_.update(wheels_vel, dt)); 154 | ASSERT_NEAR(odom_dead_reckoning_.getBodyVelocity().vx, 1.0, TOLERANCE); 155 | ASSERT_NEAR(odom_dead_reckoning_.getBodyVelocity().vy, 1.0, TOLERANCE); 156 | ASSERT_NEAR(odom_dead_reckoning_.getBodyVelocity().omega, 1.0, TOLERANCE); 157 | 158 | EXPECT_NEAR(odom_dead_reckoning_.getPose().x, 0.104873, TOLERANCE); 159 | EXPECT_NEAR(odom_dead_reckoning_.getPose().y, 0.094877, TOLERANCE); 160 | EXPECT_NEAR(odom_dead_reckoning_.getPose().theta, 0.1, TOLERANCE); 161 | 162 | odom_dead_reckoning_.update(wheels_vel, dt); 163 | EXPECT_NEAR(odom_dead_reckoning_.getPose().x, 0.218694, TOLERANCE); 164 | EXPECT_NEAR(odom_dead_reckoning_.getPose().y, 0.178810, TOLERANCE); 165 | EXPECT_NEAR(odom_dead_reckoning_.getPose().theta, 0.2, TOLERANCE); 166 | 167 | odom_dead_reckoning_.update(wheels_vel, dt); 168 | EXPECT_NEAR(odom_dead_reckoning_.getPose().x, 0.340326, TOLERANCE); 169 | EXPECT_NEAR(odom_dead_reckoning_.getPose().y, 0.250961, TOLERANCE); 170 | EXPECT_NEAR(odom_dead_reckoning_.getPose().theta, 0.3, TOLERANCE); 171 | 172 | odom_dead_reckoning_.reset(); 173 | 174 | EXPECT_NEAR(odom_dead_reckoning_.getPose().x, 0, TOLERANCE); 175 | EXPECT_NEAR(odom_dead_reckoning_.getPose().y, 0, TOLERANCE); 176 | EXPECT_NEAR(odom_dead_reckoning_.getPose().theta, 0, TOLERANCE); 177 | } 178 | --------------------------------------------------------------------------------