Add omni_wheel_drive_controller

doc/controllers_index.rst

@@ -29,6 +29,7 @@ Controllers for Wheeled Mobile Robots
 
    Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst>
    Mecanum Drive Controllers <../mecanum_drive_controller/doc/userdoc.rst>
+   Multi Omni Wheel Drive Controller <../multi_omni_wheel_drive_controller/doc/userdoc.rst>
    Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst>
    Tricycle Controller <../tricycle_controller/doc/userdoc.rst>
 

doc/images/omni_wheel_omnidirectional_drive.drawio

@@ -0,0 +1,148 @@
+<mxfile host="Electron" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/26.0.9 Chrome/128.0.6613.186 Electron/32.2.5 Safari/537.36" version="26.0.9">
+  <diagram name="Page-1" id="oXK_xTiI5W0cO1z_g1WC">
+    <mxGraphModel dx="398" dy="233" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="850" pageHeight="1100" math="1" shadow="0">
+      <root>
+        <mxCell id="0" />
+        <mxCell id="1" parent="0" />
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-35" value="" style="whiteSpace=wrap;html=1;shape=mxgraph.basic.octagon2;align=center;verticalAlign=middle;dx=15;fillColor=#e1d5e7;strokeColor=#9673a6;" parent="1" vertex="1">
+          <mxGeometry x="360" y="400" width="110" height="100" as="geometry" />
+        </mxCell>
+        <mxCell id="ChEq4ZLzQJYhXQesgG-w-1" value="" style="endArrow=classic;html=1;rounded=0;" parent="1" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="680" y="600" as="sourcePoint" />
+            <mxPoint x="680" y="520" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="ChEq4ZLzQJYhXQesgG-w-2" value="" style="endArrow=classic;html=1;rounded=0;" parent="1" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="680" y="600" as="sourcePoint" />
+            <mxPoint x="600" y="600" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-1" value="$$x_w$$" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;" parent="1" vertex="1">
+          <mxGeometry x="630" y="520" width="60" height="30" as="geometry" />
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-2" value="$$y_w$$" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;" parent="1" vertex="1">
+          <mxGeometry x="590" y="600" width="60" height="30" as="geometry" />
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-39" value="" style="rounded=1;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;rotation=-45;direction=north;" parent="1" vertex="1">
+          <mxGeometry x="450" y="380" width="20" height="60" as="geometry" />
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-26" value="" style="endArrow=classic;html=1;rounded=0;" parent="1" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="420" y="450" as="sourcePoint" />
+            <mxPoint x="420" y="320" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-27" value="" style="endArrow=classic;html=1;rounded=0;" parent="1" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="420" y="450" as="sourcePoint" />
+            <mxPoint x="280" y="450" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-28" value="$$x_b$$" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;" parent="1" vertex="1">
+          <mxGeometry x="370" y="320" width="60" height="30" as="geometry" />
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-30" value="$$y_b$$" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;" parent="1" vertex="1">
+          <mxGeometry x="270" y="450" width="60" height="30" as="geometry" />
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-37" value="" style="rounded=1;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;rotation=-45;" parent="1" vertex="1">
+          <mxGeometry x="338.54" y="400" width="60" height="20" as="geometry" />
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-47" value="" style="endArrow=classic;html=1;rounded=0;fillColor=#e51400;strokeColor=#B20000;" parent="1" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="470" y="420" as="sourcePoint" />
+            <mxPoint x="490" y="440" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-48" value="" style="rounded=1;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;rotation=-225;" parent="1" vertex="1">
+          <mxGeometry x="430" y="480" width="60" height="20" as="geometry" />
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-49" value="" style="endArrow=classic;html=1;rounded=0;fillColor=#e51400;strokeColor=#B20000;" parent="1" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="450" y="500" as="sourcePoint" />
+            <mxPoint x="430" y="520" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-50" value="" style="endArrow=classic;html=1;rounded=0;fillColor=#e51400;strokeColor=#B20000;" parent="1" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="380" y="400" as="sourcePoint" />
+            <mxPoint x="400" y="380" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-53" value="" style="rounded=1;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;rotation=-135;" parent="1" vertex="1">
+          <mxGeometry x="338.54" y="480" width="60" height="20" as="geometry" />
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-54" value="" style="endArrow=classic;html=1;rounded=0;fillColor=#e51400;strokeColor=#B20000;" parent="1" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="360" y="480" as="sourcePoint" />
+            <mxPoint x="340" y="460" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-58" value="" style="verticalLabelPosition=bottom;verticalAlign=top;html=1;shape=mxgraph.basic.arc;startAngle=0.7182744825694465;endAngle=0.12992074927787245;strokeWidth=2;fillColor=none;strokeColor=#0e8088;" parent="1" vertex="1">
+          <mxGeometry x="360" y="355" width="70" height="60" as="geometry" />
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-59" value="" style="endArrow=classic;html=1;rounded=0;exitX=0.005;exitY=0.57;exitDx=0;exitDy=0;exitPerimeter=0;fillColor=#b1ddf0;strokeColor=#0E8088;strokeWidth=2;" parent="1" source="rHnyt-oSxQMcdHQplmVE-58" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="260" y="400" as="sourcePoint" />
+            <mxPoint x="365" y="400" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-60" value="$$\gamma$$" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontColor=#0E8088;" parent="1" vertex="1">
+          <mxGeometry x="330" y="340" width="60" height="30" as="geometry" />
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-62" value="" style="endArrow=classic;startArrow=classic;rounded=0;strokeColor=#6c8ebf;fillColor=#dae8fc;fontColor=none;noLabel=1;strokeWidth=2;" parent="1" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="420" y="430" as="sourcePoint" />
+            <mxPoint x="420" y="390" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-63" value="&lt;font&gt;$$v_b,_x$$&lt;/font&gt;" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontColor=#6C8EBF;fontSize=12;" parent="1" vertex="1">
+          <mxGeometry x="385" y="400" width="40" height="15" as="geometry" />
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-64" value="" style="endArrow=classic;startArrow=classic;rounded=0;strokeColor=#6C8EBF;fontColor=none;noLabel=1;fillColor=#d5e8d4;strokeWidth=2;" parent="1" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="350" y="450" as="sourcePoint" />
+            <mxPoint x="390" y="450" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-65" value="&lt;font&gt;$$v_b,_y$$&lt;/font&gt;" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontColor=#6C8EBF;fontSize=12;" parent="1" vertex="1">
+          <mxGeometry x="358.54" y="450" width="40" height="15" as="geometry" />
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-69" value="" style="verticalLabelPosition=bottom;verticalAlign=top;shape=mxgraph.basic.arc;startAngle=0.5737918088252166;endAngle=0.1;noLabel=1;fillColor=none;strokeColor=#6C8EBF;strokeWidth=2;" parent="1" vertex="1">
+          <mxGeometry x="405" y="440" width="25" height="20" as="geometry" />
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-73" value="" style="endArrow=classic;rounded=0;exitX=0.248;exitY=0.91;exitDx=0;exitDy=0;exitPerimeter=0;strokeColor=#6C8EBF;fillColor=#d5e8d4;fontColor=none;noLabel=1;" parent="1" source="rHnyt-oSxQMcdHQplmVE-69" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="420" y="490" as="sourcePoint" />
+            <mxPoint x="418" y="460" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-75" value="&lt;font&gt;$$w_b,_z$$&lt;/font&gt;" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontColor=#6C8EBF;fontSize=12;" parent="1" vertex="1">
+          <mxGeometry x="380" y="425" width="40" height="15" as="geometry" />
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-76" value="" style="endArrow=classic;html=1;rounded=0;entryX=0.5;entryY=1;entryDx=0;entryDy=0;strokeColor=#0E8088;strokeWidth=2;" parent="1" target="rHnyt-oSxQMcdHQplmVE-48" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="420" y="450" as="sourcePoint" />
+            <mxPoint x="470" y="420" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-77" value="$$R$$" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontColor=#0E8088;" parent="1" vertex="1">
+          <mxGeometry x="410" y="440" width="60" height="30" as="geometry" />
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-78" value="" style="verticalLabelPosition=bottom;verticalAlign=top;html=1;shape=mxgraph.basic.arc;startAngle=0.38408468867550993;endAngle=0.02988324191219446;rotation=-165;fillColor=none;strokeColor=#0E8088;strokeWidth=2;" parent="1" vertex="1">
+          <mxGeometry x="460" y="410" width="64.77" height="81.33" as="geometry" />
+        </mxCell>
+        <mxCell id="rHnyt-oSxQMcdHQplmVE-80" value="$$\theta$$" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontColor=#0E8088;" parent="1" vertex="1">
+          <mxGeometry x="510" y="430" width="60" height="30" as="geometry" />
+        </mxCell>
+        <mxCell id="xJ0_dUwsu9lpXLYQ-wWM-2" value="" style="endArrow=classic;html=1;rounded=0;exitX=0.722;exitY=0.945;exitDx=0;exitDy=0;exitPerimeter=0;strokeColor=#0E8088;entryX=0.224;entryY=0.912;entryDx=0;entryDy=0;entryPerimeter=0;" parent="1" source="rHnyt-oSxQMcdHQplmVE-78" target="rHnyt-oSxQMcdHQplmVE-39" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="488" y="411" as="sourcePoint" />
+            <mxPoint x="476" y="420" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+      </root>
+    </mxGraphModel>
+  </diagram>
+</mxfile>

doc/mobile_robot_kinematics.rst

@@ -23,7 +23,92 @@ The forward integration of the kinematic model using the encoders of the wheel a
 Omnidirectional Wheeled Mobile Robots
 .....................................
 
-Robots with omniwheels or mecanum wheels. Section will be updated if controllers for these robots are implemented.
+Omnidirectional Drive Robots using Omni Wheels
+,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
+
+The below explains the kinematics of omnidirectional drive robots using 3 or more omni wheels.
+It follows the coordinate conventions defined in `ROS REP 103 <https://www.ros.org/reps/rep-0103.html>`__.
+
+.. image:: images/omni_wheel_omnidirectional_drive.svg
+   :width: 550
+   :align: center
+   :alt: Omnidirectional Drive Robot using Omni Wheels
+
+* :math:`x_b,y_b` is the robot's body-frame coordinate system, located at the contact point of the wheel on the ground.
+* :math:`x_w,y_w` is the world coordinate system.
+* :math:`v_{b,x},` is the robot's linear velocity on the x-axis.
+* :math:`v_{b,y}` is the robot's linear velocity on the y-axis.
+* :math:`w_{b,z}` is the robot's angular velocity on the z-axis.
+* :math:`R` is the robot's radius / the distance between the robot's center and the wheels.
+* Red arrows on the wheels signify the positive direction of their rotation.
+* :math:`\gamma` is the angular offset of the first wheel from :math:`x_b`.
+* :math:`\theta` is the angle between each wheel which can be calculated using the below equation where :math:`n` is the number of wheels.
+
+.. math::
+
+   θ = \frac{2\pi}{n}
+
+**Inverse Kinematics**
+
+The necessary angular velocity of the wheels to achieve a desired body twist can be calculated using the below matrix:
+
+.. math::
+
+  A =
+  \begin{bmatrix}
+    sin(\gamma) & -cos(\gamma) & -R  \\
+    sin(\theta + \gamma) & -cos(\theta + \gamma) & -R\\
+    sin(2\theta + \gamma) & -cos(2\theta + \gamma) & -R\\
+    sin(3\theta + \gamma) & -cos(3\theta + \gamma) & -R\\
+    \vdots & \vdots & \vdots\\
+    sin((n-1)\theta + \gamma) & -cos((n-1)\theta + \gamma) & -R\\
+  \end{bmatrix}
+
+.. math::
+
+  \begin{bmatrix}
+    w_1\\
+    w_2\\
+    w_3\\
+    w_4\\
+    \vdots\\
+    w_n
+  \end{bmatrix} =
+  \frac{1}{r}
+  A
+  \begin{bmatrix}
+    v_{b,x}\\
+    v_{b,y}\\
+    w_{b,z}\\
+  \end{bmatrix}
+
+Here :math:`w_1..w_n` are the angular velocities of the wheels and :math:`r` is the radius of the wheels.
+These equations can be written in algebraic form for any wheel :math:`i` like this:
+
+.. math::
+  \omega_i = \frac{\sin((i-1)\theta + \gamma) v_{b,x} - \cos((i-1)\theta + \gamma) v_{b,y} - R w_{b,z}}{r}
+
+**Forward Kinematics**
+
+The body twist of the robot can be obtained from the wheel velocities by using the pseudoinverse of matrix :math:`A`.
+
+.. math::
+
+  \begin{bmatrix}
+    v_{b,x}\\
+    v_{b,y}\\
+    w_{b,z}\\
+  \end{bmatrix} =
+  A^+
+  (r
+  \begin{bmatrix}
+    w_1\\
+    w_2\\
+    w_3\\
+    w_4\\
+    \vdots\\
+    w_n
+  \end{bmatrix})
 
 Nonholonomic Wheeled Mobile Robots
 .....................................

doc/release_notes.rst

@@ -57,6 +57,10 @@ mecanum_drive_controller
 ************************
 * 🚀 The mecanum_drive_controller was added 🎉 (`#512 <https://github.com/ros-controls/ros2_controllers/pull/512>`_).
 
+multi_omni_wheel_drive_controller
+*********************************
+* 🚀 The multi_omni_wheel_drive_controller was added 🎉 (`#1535 <https://github.com/ros-controls/ros2_controllers/pull/1535>`_).
+
 pid_controller
 ************************
 * 🚀 The PID controller was added 🎉 (`#434 <https://github.com/ros-controls/ros2_controllers/pull/434>`_).

multi_omni_wheel_drive_controller/CMakeLists.txt

@@ -0,0 +1,99 @@
+cmake_minimum_required(VERSION 3.16)
+project(multi_omni_wheel_drive_controller)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable
+                      -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
+                      -Werror=missing-braces)
+endif()
+
+# Find dependencies
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  ament_cmake
+  controller_interface
+  generate_parameter_library
+  geometry_msgs
+  hardware_interface
+  nav_msgs
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+  tf2
+  tf2_msgs
+  Eigen3
+)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+generate_parameter_library(
+  ${PROJECT_NAME}_parameters
+  src/${PROJECT_NAME}_parameters.yaml
+)
+
+add_library(${PROJECT_NAME} SHARED
+  src/${PROJECT_NAME}.cpp
+  src/odometry.cpp
+)
+target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)
+target_include_directories(${PROJECT_NAME}
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+)
+target_link_libraries(${PROJECT_NAME} PRIVATE
+  ${PROJECT_NAME}_parameters
+  Eigen3::Eigen
+)
+ament_target_dependencies(${PROJECT_NAME} PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
+pluginlib_export_plugin_description_file(controller_interface multi_omni_wheel_drive_plugin.xml)
+
+# Install
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}/
+)
+install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_parameters
+  EXPORT export_${PROJECT_NAME}
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  ament_add_gmock(test_${PROJECT_NAME} test/test_${PROJECT_NAME}.cpp)
+  target_link_libraries(test_${PROJECT_NAME}
+    ${PROJECT_NAME}
+    ${PROJECT_NAME}_parameters
+    Eigen3::Eigen
+  )
+  ament_target_dependencies(test_${PROJECT_NAME}
+    geometry_msgs
+    hardware_interface
+    nav_msgs
+    rclcpp
+    rclcpp_lifecycle
+    realtime_tools
+    tf2
+    tf2_msgs
+    Eigen3
+  )
+
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+  ament_add_gmock(test_load_${PROJECT_NAME} test/test_load_${PROJECT_NAME}.cpp)
+  ament_target_dependencies(test_load_${PROJECT_NAME}
+    controller_manager
+    ros2_control_test_assets
+  )
+
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()