Added Swerve Drive Controller Package #1694

nitin2606 · 2025-05-21T11:17:38Z

Description
This PR introduces the swerve_drive_controller, a new controller for swerve drive robots with four independently steerable wheels, enabling omnidirectional motion in ROS 2. It complements controllers like diff_drive_controller by supporting advanced mobile robot platforms.

Features

Supports geometry_msgs/msg/Twist or TwistStamped velocity inputs (x, y linear; z angular).
Publishes raw nav_msgs/msg/Odometry for user-defined post-processing.
Publishes /tf transforms (optional, if enable_odom_tf=true).
Configurable via YAML for wheel geometry and kinematic constraints.
Changes
Added swerve_drive_controller package (src/, include/, test/).
Exported as a pluginlib plugin (swerve_drive_controller_plugin.xml).
Updated ros2_controllers CMakeLists.txt and package.xml.
Included gtest tests (test/test_swerve_drive_controller.cpp) and config (test/config/test_swerve_drive_controller.yaml).

christophfroehlich

Thanks again for your contribution. Could you please fix the failing tests? A very quick first review below.

Do you mind add a brief section in the kinematics section of the docs, even in a new PR to get it merged faster? Thanks!

swerve_drive_controller/CMakeLists.txt

swerve_drive_controller/LICENSE

swerve_drive_controller/doc/userdoc.rst

swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp

swerve_drive_controller/include/swerve_drive_controller/swerve_drive_kinematics.hpp

swerve_drive_controller/package.xml

swerve_drive_controller/src/swerve_drive_controller.cpp

swerve_drive_controller/src/swerve_drive_kinematics.cpp

swerve_drive_controller/test/test_swerve_drive_controller.hpp

swerve_drive_controller/test/test_swerve_drive_controller.cpp

nitin2606 · 2025-05-22T05:50:46Z

I'll make changes and push.

…s.txt of swerve_drive_controller package Signed-off-by: nitin <nitinmaurya2606@gmail.com>

christophfroehlich · 2025-05-27T20:33:06Z

do you mind cherry-picking the changes to the docs in a separate PR? Splitting this up would help to reviewing things.

nitin2606 · 2025-05-27T22:26:08Z

do you mind cherry-picking the changes to the docs in a separate PR? Splitting this up would help to reviewing things.

I have raised a separate PR for changes in docs. PR #1712

doc/controllers_index.rst

Maverobot

Hi there, thanks for your contribution! I've started reviewing the changes and have left a few comments. I will try to find time to work through it and will follow up once I'm done.

swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp

swerve_drive_controller/src/swerve_drive_kinematics.cpp

swerve_drive_controller/src/swerve_drive_controller.cpp

swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp

swerve_drive_controller/src/swerve_drive_controller.cpp

swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp

doc/mobile_robot_kinematics.rst

Co-authored-by: Zheng Qu <quzhengrobot@gmail.com>

mergify · 2025-06-24T17:03:08Z

This pull request is in conflict. Could you fix it @nitin2606?

Signed-off-by: nitin <nitinmaurya2606@gmail.com>

Narukara · 2025-09-30T07:40:11Z

There's another issue: the userdoc.rst mentions that the use_stamped_vel option can be used to configure the controller to accept either TwistStamped or Twist messages. However, this does not appear to be implemented in the code — the controller always uses TwistStamped. 🤔

nitin2606 · 2025-09-30T08:17:45Z

There's another issue: the userdoc.rst mentions that the use_stamped_vel option can be used to configure the controller to accept either TwistStamped or Twist messages. However, this does not appear to be implemented in the code — the controller always uses TwistStamped. 🤔

userdoc.rst is not updated. All the controllers of ros2_control have switched to TwistStamped only.

computed from wheelbase, trackwidth, offset_x, and offset_y. This improves flexibility and reduces duplication when changing robot dimensions. Signed-off-by: nitin <nitinmaurya2606@gmail.com>

Narukara

I'm not sure if this is the right place to handle the wheel radius. I tested the modified controller using gazebo, and both the controls and odometry seem to work fine. By the way, I used the humble version, so I made some necessary changes.

Narukara · 2025-10-07T08:49:51Z

swerve_drive_controller/src/swerve_drive_controller.cpp

+  auto logger = get_node()->get_logger();
+
+  RCLCPP_INFO(logger, "Updated Kinematics");


Suggested change

auto logger = get_node()->get_logger();

RCLCPP_INFO(logger, "Updated Kinematics");

The controller prints a lot of this when it runs, which is a bit annoying.

Narukara · 2025-10-07T08:50:26Z

swerve_drive_controller/src/swerve_drive_controller.cpp

+        wheel_joint_names[i]);
+    }
+    axle_handles_[i]->set_position(wheel_command[i].steering_angle);
+    wheel_handles_[i]->set_velocity(wheel_command[i].drive_velocity);


Suggested change

wheel_handles_[i]->set_velocity(wheel_command[i].drive_velocity);

wheel_handles_[i]->set_velocity(wheel_command[i].drive_velocity / params_.wheel_radius);

Solved in latest commit

Narukara · 2025-10-07T08:50:59Z

swerve_drive_controller/src/swerve_drive_controller.cpp

+    }
+    else
+    {
+      velocity_array[i] = wheel_handles_[i]->get_feedback();


Suggested change

velocity_array[i] = wheel_handles_[i]->get_feedback();

velocity_array[i] = wheel_handles_[i]->get_feedback() * params_.wheel_radius;

Solved in latest commit

Narukara · 2025-10-07T09:04:01Z

Another small issue: I found that when I stop sending speed commands, the controller automatically straightens all the wheels. Maybe it would be more reasonable to keep each wheel at its current steering angle?

- Updated kinematics to compute angular velocity for wheels instead of linear velocity for more accurate motion control. - Enhanced controller to retain the last steering angle when stopped, preventing the wheels from snapping back to zero orientation.

nitin2606 · 2025-10-07T22:24:24Z

Another small issue: I found that when I stop sending speed commands, the controller automatically straightens all the wheels. Maybe it would be more reasonable to keep each wheel at its current steering angle?

please check latest commit

Narukara · 2025-10-08T07:25:33Z

swerve_drive_controller/src/swerve_drive_controller.cpp

+  if (realtime_odometry_publisher_)
+  {
+    auto & odometry_message = realtime_odometry_publisher_->msg_;
+    odometry_message.header.stamp = time;
+    odometry_message.pose.pose.position.x = odometry_.x;
+    odometry_message.pose.pose.position.y = odometry_.y;
+    odometry_message.pose.pose.orientation.x = orientation.x();
+    odometry_message.pose.pose.orientation.y = orientation.y();
+    odometry_message.pose.pose.orientation.z = orientation.z();
+    odometry_message.pose.pose.orientation.w = orientation.w();
+    realtime_odometry_publisher_->tryPublish(odometry_message);
+  }


The odometry message published here does not contain twist information (twist.linear.x, twist.linear.y, twist.angular.z). I have confirmed this issue in the Gazebo test.

Narukara

add odometry twist

swerve_drive_controller/include/swerve_drive_controller/swerve_drive_kinematics.hpp

swerve_drive_controller/src/swerve_drive_controller.cpp

swerve_drive_controller/src/swerve_drive_kinematics.cpp

…_drive_kinematics.hpp Co-authored-by: Narukara <narukara17@gmail.com>

Co-authored-by: Narukara <narukara17@gmail.com>

Signed-off-by: nitin <nitinmaurya2606@gmail.com>

Narukara · 2025-10-14T07:28:45Z

I've found another tricky problem. When I send the command tw.linear.x = -0.5, I expect the car to move backward. But what actually happens is that it first rotates the wheels 180 degrees and then moves forward, as shown in this video.

2025.10.14.15.21.35.webm

nitin2606 · 2025-10-14T07:40:53Z

I've found another tricky problem. When I send the command tw.linear.x = -0.5, I expect the car to move backward. But what actually happens is that it first rotates the wheels 180 degrees and then moves forward, as shown in this video.

2025.10.14.15.21.35.webm

This appears to be a kinematics issue — when the robot is commanded to move backward, the wheels shouldn’t rotate; they should simply translate backward.

christophfroehlich · 2025-10-14T14:57:32Z

I've found another tricky problem. When I send the command tw.linear.x = -0.5, I expect the car to move backward. But what actually happens is that it first rotates the wheels 180 degrees and then moves forward, as shown in this video.
2025.10.14.15.21.35.webm

This appears to be a kinematics issue — when the robot is commanded to move backward, the wheels shouldn’t rotate; they should simply translate backward.

This is a "feature" of the solution of the inverse kinematics not being unique. But this could be handled to have a best effort behavior. Having a look a the video, there should also be some master-slave behavior of the steering and speed command of the wheels be employed, see the reduce_wheel_speed_until_steering_reached parameter here:

https://github.com/ros-controls/ros2_controllers/blob/d7a699fd82acd90a54b8a28f809d29d705968ff6/steering_controllers_library/src/steering_odometry.cpp#L263C21-L284

Signed-off-by: nitin <nitinmaurya2606@gmail.com>

nitin2606 · 2025-10-14T21:58:26Z

I've found another tricky problem. When I send the command tw.linear.x = -0.5, I expect the car to move backward. But what actually happens is that it first rotates the wheels 180 degrees and then moves forward, as shown in this video.
2025.10.14.15.21.35.webm

This appears to be a kinematics issue — when the robot is commanded to move backward, the wheels shouldn’t rotate; they should simply translate backward.

This is a "feature" of the solution of the inverse kinematics not being unique. But this could be handled to have a best effort behavior. Having a look a the video, there should also be some master-slave behavior of the steering and speed command of the wheels be employed, see the reduce_wheel_speed_until_steering_reached parameter here:

https://github.com/ros-controls/ros2_controllers/blob/d7a699fd82acd90a54b8a28f809d29d705968ff6/steering_controllers_library/src/steering_odometry.cpp#L263C21-L284

Please check the latest commit, I have tried to fix the issue

BCaran · 2025-10-16T13:42:57Z

I've found another tricky problem. When I send the command tw.linear.x = -0.5, I expect the car to move backward. But what actually happens is that it first rotates the wheels 180 degrees and then moves forward, as shown in this video.
2025.10.14.15.21.35.webm

This appears to be a kinematics issue — when the robot is commanded to move backward, the wheels shouldn’t rotate; they should simply translate backward.

This is a "feature" of the solution of the inverse kinematics not being unique. But this could be handled to have a best effort behavior. Having a look a the video, there should also be some master-slave behavior of the steering and speed command of the wheels be employed, see the reduce_wheel_speed_until_steering_reached parameter here:

https://github.com/ros-controls/ros2_controllers/blob/d7a699fd82acd90a54b8a28f809d29d705968ff6/steering_controllers_library/src/steering_odometry.cpp#L263C21-L284

Yes, that should be considered a feature. In my controller (which is not implemented using the 'ros2_control' framework, just normal node), I enforce steering angle limits of ±π/2.

void optimiseSpeedAngle(double returnSpeedAngle[], double speed, double angle) const{ 
            if(angle > (PI/2)){
                returnSpeedAngle[0] = -1 * speed;
                returnSpeedAngle[1] = angle - PI;
            }
            else if (angle <= (-PI/2)){
                returnSpeedAngle[0] = -1 * speed;
                returnSpeedAngle[1] = angle + PI;
            }
            else{
                returnSpeedAngle[0] = speed;
                returnSpeedAngle[1] = angle; 
            }
        }

When the inverse kinematics produces an angle outside these limits, I simply add π to the steering angle and reverse the wheel’s driving direction. In the video case, this means the robot would just reverse the direction of the driving wheel.

I’ve also seen some real-world implementations where, if the steering angle change is larger than a certain threshold, the driving wheels wait until the steering angle is reached—effectively introducing a short delay, similar to dead time in control system theory.

However, this can become a “problem” if the path contains discontinuities or sharp turns. To avoid this, it is often better to generate smoother, polynomial-based paths that ensure continuous steering without large jumps.

tgaspar · 2025-10-29T14:12:14Z

Hi!

First and foremost - thank you for opening this PR and adding this controller! Much appreciated.

I have played around with it with a robot that I am currently working integrating this controller was super smooth! I did find a couple of things I wanted to check with you and maybe suggest a bit of parametrization.

Steering axis direction

The URDF was created with the mindset that the turning axis of the motor is positive towards the next motor in the chain. In the case of the steering motors this means that it points downwards (see image).

However, my understanding of the equations provided in the documentation and empiric tests is that this should not be like this. My understanding is, that the axis should point upwards (aligned with the base link Z axis direction). If I left the axis as shown in the picture, the robot would turn the wheels like this when rotating:

If I then changed the sign of the axis of rotation of the joint (from <axis xyz="0 0 1"/> to <axis xyz="0 0 -1"/>) it worked well (the red arrow denotes the direction of the spinning):

My suggestion (or question) would therefore be: is it reasonable to add a parameter to the controller configuration invert_steer_motors: True/False (or something similar) to account for this?

Steering motors limits

Changing the direction of the linear component while the rotational component is untouched can lead to the steering motors hitting their limit and staying there.

In the video below, I first set all the velocities to zero. I then set the rotation to a relatively small value and then proceed to add linear velocity. You can see that when I go into the positive direction, the two wheels on the right turn 90 degrees (the limit of the steering motors), when I go into negative direction, the left wheels do that.

swerve_drive_issue.mp4

I think this is slightly problematic as it this situation can occur very quickly with steering motors of a limited range.

Again, thank you for all the work and I hope my report brings some value 🙂

Narukara · 2025-11-02T09:23:25Z

The steering_controllers_library documentation states:

The orientation of the steering axes is expected as follows: When a positive steering position value is commanded, the robot should turn in the positive direction of the z-axis of the vehicle (see REP-103)."

I'm unsure how this applies to a four-wheel independent steering vehicle. In the vehicle model I'm using (which works correctly), the steering mechanism is defined as follows:

  <!-- steer macro -->
  <xacro:macro name="steer" params="name x y z">
    <link name="${name}">
      <inertial>
        <mass value="0.0001"/>
        <inertia ixx="0.000001" ixy="0.0" ixz="0.0" iyy="0.000001" iyz="0.0" izz="0.000001" />
      </inertial>
    </link>
    <joint name="${name}_joint" type="continuous">
      <origin xyz="${x} ${y} ${z}" rpy="0 0 0" />
      <parent link="chassis" />
      <child link="${name}" />
      <axis xyz="0 0 1" />
      <dynamics damping="0.0" />
    </joint>
  </xacro:macro>

Its Z-axis points upwards, and it rotates around the Z-axis. In fact, I'm mimicking the design of the Ackerman vehicle here.

Narukara · 2025-11-04T12:24:22Z

I think this is slightly problematic as it this situation can occur very quickly with steering motors of a limited range.

The control of a four-wheel independent steering vehicle with steering angle limitations might be a rather complex problem. Personally, I suggest that we only complete the basic functions in this PR and leave this problem to the next issue/PR. 🙂

tgaspar · 2025-11-04T12:41:34Z

Thank you @Narukara for the feedback.

The link to the specifications/documentation of the steering mechanism regarding the Z axis is very useful. I have a similar definition of the steering joint as yours, it's just that its parent frame Z axis points downwards 🙂 Subsequently also the steering axis points down.

Given that this is an isolated issue, I understand that it does not need to make it into the PR.

As for the limited rotation of the steering axes: totally agree! If it's a complex problem to solve it can be adjusted iteratively later. In this case, I would find it reasonable to note this limitation somewhere.

Narukara · 2025-11-07T09:43:10Z

Just wanted to mention that this PR introduced the RealtimeThreadSafeBox starting from commit 37a2ece ("Made swerve_drive_controller chainable"), which is not present in the Humble version. I encountered this issue while porting. So, will this controller be supported in the Humble version?

christophfroehlich · 2025-11-07T09:59:28Z

I encountered this issue while porting. So, will this controller be supported in the Humble version?

Maybe not. But you can always compile the rolling version on humble distro, see "development version".

nitin2606 and others added 2 commits May 21, 2025 16:34

Add swerve_drive_controller package with tests and documentation

51c5c08

Merge branch 'ros-controls:master' into feature/swerve-drive-controller

2cb3f0f

github-actions bot requested review from Maverobot, anfemosa, bijoua29, bmagyar, harderthan, mhubii and saikishor May 21, 2025 11:17

nitin2606 mentioned this pull request May 21, 2025

Add Swerve Drive Controller #1692

Closed

christophfroehlich requested changes May 21, 2025

View reviewed changes

christophfroehlich linked an issue May 21, 2025 that may be closed by this pull request

Add a controller for afour wheel independent steering / drive mobile base, aka a swerve drive #1425

Open

nitin2606 and others added 2 commits May 25, 2025 22:21

Merge branch 'ros-controls:master' into feature/swerve-drive-controller

89c269d

Added Swerve Drive In mobile_robot_kinematics.rst and fixed CMakeList…

a6b6ac7

…s.txt of swerve_drive_controller package Signed-off-by: nitin <nitinmaurya2606@gmail.com>

nitin2606 requested a review from christophfroehlich May 27, 2025 20:01

Merge branch 'ros-controls:master' into feature/swerve-drive-controller

a146ff8

nitin2606 mentioned this pull request May 27, 2025

Added Swerve Drive Kinematics in mobile_robot_kinematics.rst #1712

Merged

Merge branch 'master' into feature/swerve-drive-controller

369bfd3

bmagyar reviewed Jun 5, 2025

View reviewed changes

doc/controllers_index.rst Outdated Show resolved Hide resolved

Apply suggestions from code review

647b161

Maverobot reviewed Jun 5, 2025

View reviewed changes

doc/mobile_robot_kinematics.rst Show resolved Hide resolved

This was referenced Jun 23, 2025

Added Swerve Drive Kinematics in mobile_robot_kinematics.rst (backport #1712) #1770

Merged

Added Swerve Drive Kinematics in mobile_robot_kinematics.rst (backport #1712) #1771

Merged

Update swerve_drive_controller/src/swerve_drive_controller.cpp

8b72f06

Co-authored-by: Zheng Qu <quzhengrobot@gmail.com>

Added Swerve Drive Kinematics in mobile_robot_kinematics.rst

ee8d704

Signed-off-by: nitin <nitinmaurya2606@gmail.com>

Instead of hardcoding wheel positions, wheel coordinates are now

bc5aa20

computed from wheelbase, trackwidth, offset_x, and offset_y. This improves flexibility and reduces duplication when changing robot dimensions. Signed-off-by: nitin <nitinmaurya2606@gmail.com>

nitin2606 requested a review from christophfroehlich September 30, 2025 18:52

Narukara reviewed Oct 7, 2025

View reviewed changes

Merge branch 'master' into feature/swerve-drive-controller

e67464d

Narukara reviewed Oct 8, 2025

View reviewed changes

Narukara suggested changes Oct 10, 2025

View reviewed changes

nitin2606 and others added 6 commits October 10, 2025 13:36

Update swerve_drive_controller/include/swerve_drive_controller/swerve…

a56aa93

…_drive_kinematics.hpp Co-authored-by: Narukara <narukara17@gmail.com>

Update swerve_drive_controller/src/swerve_drive_controller.cpp

df62ca8

Co-authored-by: Narukara <narukara17@gmail.com>

Update swerve_drive_controller/src/swerve_drive_kinematics.cpp

cf0a3d0

Co-authored-by: Narukara <narukara17@gmail.com>

Update swerve_drive_controller/src/swerve_drive_kinematics.cpp

631e97c

Co-authored-by: Narukara <narukara17@gmail.com>

Made swerve_drive_controller chainable

37a2ece

Signed-off-by: nitin <nitinmaurya2606@gmail.com>

Suppressed false syntax error

4674fa7

Signed-off-by: nitin <nitinmaurya2606@gmail.com>

nitin2606 requested a review from Narukara October 13, 2025 20:41

choose minimal steering rotation and reverse drive when needed

c5ce36d

Signed-off-by: nitin <nitinmaurya2606@gmail.com>

		auto logger = get_node()->get_logger();

		RCLCPP_INFO(logger, "Updated Kinematics");

	wheel_handles_[i]->set_velocity(wheel_command[i].drive_velocity);
	wheel_handles_[i]->set_velocity(wheel_command[i].drive_velocity / params_.wheel_radius);

	velocity_array[i] = wheel_handles_[i]->get_feedback();
	velocity_array[i] = wheel_handles_[i]->get_feedback() * params_.wheel_radius;

Added Swerve Drive Controller Package #1694

Are you sure you want to change the base?

Added Swerve Drive Controller Package #1694

Uh oh!

Conversation

nitin2606 commented May 21, 2025

Uh oh!

christophfroehlich left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

nitin2606 commented May 22, 2025

Uh oh!

christophfroehlich commented May 27, 2025

Uh oh!

nitin2606 commented May 27, 2025

Uh oh!

Uh oh!

Maverobot left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

mergify bot commented Jun 24, 2025

Uh oh!

Narukara commented Sep 30, 2025

Uh oh!

nitin2606 commented Sep 30, 2025

Uh oh!

Narukara left a comment

Choose a reason for hiding this comment

Uh oh!

Narukara Oct 7, 2025

Choose a reason for hiding this comment

Uh oh!

Narukara Oct 7, 2025

Choose a reason for hiding this comment

Uh oh!

nitin2606 Oct 7, 2025

Choose a reason for hiding this comment

Uh oh!

Narukara Oct 7, 2025

Choose a reason for hiding this comment

Uh oh!

nitin2606 Oct 7, 2025

Choose a reason for hiding this comment

Uh oh!

Narukara commented Oct 7, 2025

Uh oh!

nitin2606 commented Oct 7, 2025

Uh oh!

Narukara Oct 8, 2025

Choose a reason for hiding this comment

Uh oh!

Narukara left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Narukara commented Oct 14, 2025

Uh oh!

nitin2606 commented Oct 14, 2025