Add SwerveDriveOdometry utility to gz-math

[C88-YQ]

🎉 New feature

Related to gazebosim/gz-sim#3364

Summary

This PR introduces a new SwerveDriveOdometry utility class to gz-math.

The new class provides kinematic and odometry computation support for four-wheel independent steering (swerve / 4WISD) drive systems.

Main features:

• Compute robot pose updates from steering angles and wheel displacements
• Support time-based incremental odometry updates
• Follow the design and conventions of existing odometry utilities in gz-math
• Maintain full backward compatibility (no changes to existing APIs)

This utility is intended to support swerve drive system implementations in gz-sim, while keeping kinematics and odometry logic within gz-math to preserve architectural separation between math utilities and simulation systems.

The implementation does not modify any existing behavior.

If Eigen (or any additional math utilities) are used internally, they are not exposed in the public API.

Test it

To test this feature locally:

1. Build gz-math

2. Run the unit tests (or filter to only the new test):

   • Run all tests:

     ctest --output-on-failure

   • Or run only the new SwerveDriveOdometry test:

     ctest -R SwerveDriveOdometry --output-on-failure

The new unit tests cover:

• Initialization / re-initialization behavior (BasicInit)
• Straight motion in X (StraightForward)
• Pure lateral motion in Y (90° steering) (StraightSide)
• Diagonal translation (45° steering) (StraightDiagonal)
• In-place rotation with opposing wheel motions and steering angles (RotateInPlace)

Checklist

• Signed all commits for DCO
• Added a screen capture or video to the PR description that demonstrates the feature
• Added tests
• Added example and/or tutorial
• Updated documentation (as needed)
• Updated migration guide (as needed)
• Consider updating Python bindings (if the library has them)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• Updated Bazel files (if adding new files). Created an issue otherwise.
• While waiting for a review on your PR, please help review another open pull request to support the maintainers
• Was GenAI used to generate this PR? If so, make sure to add "Generated-by" to your commits. (See this policy for more info.)

[arjo129]

This is a good first attempt, there is an edge case I've thought of that I dont think handled correctly.

The rest of the math seems to check out and co uld be genuinely useful for the communinty.

[arjo129]

Also consider re-targetting this pr to main instead of gz-math9.

[C88-YQ]

Hi @arjo129, thanks again for the detailed review!

All the suggestions have now been addressed in the latest commits.

[C88-YQ]

Hi @arjo129
I realized that I forgot to update the Bazel dependency in the previous revision. I've just pushed a fix to BUILD.bazel.

Could you please approve the workflow again so the remaining CI checks can run?

[C88-YQ]

Hi @arjo129,

I updated the Eigen Bazel dependency in BUILD.bazel.

Initially I added "@eigen//:eigen", but that triggered a failure in //:buildifier.test. I then followed the pattern used in gz-sim and changed it to "@eigen".

My local Bazel environment currently seems to have some issues, so I'm not fully confident that my local results reflect the CI environment correctly.

Could you please approve the workflows again so the remaining CI checks can run with the latest changes?
Also, if you have a recommendation on the correct way to reference Eigen in this repository, I’d really appreciate the guidance.

[arjo129]

Yeah the eigen dependency is another thing I'm going to have to look into a bit. For some reason I thought we always depended on eigen but that turns out to not be the case. FWIW I think sim itself depends on Eigen, but math does not. I'll ask in zulip about this to see the best way forward.

[C88-YQ]

Thanks for looking into this!

In my current implementation of SwerveDriveOdometry, I used Eigen’s SVD to solve the linear system that arises from the wheel velocity equations. Since the system can sometimes be overdetermined, I thought SVD would be a robust way to compute the least-squares solution. I mainly used Eigen just for solving this small linear system.

If depending on Eigen is not recommended for gz-math, solving this system without it might become a bit cumbersome. One possible option could be to adopt approaches like the one described here: https://control.ros.org/humble/doc/ros2_controllers/doc/mobile_robot_kinematics.html#omnidirectional-drive-robots-using-omni-wheels:~:text=)-,Odometry,-The%20body%20twist
However, I feel the overall robustness may not be as good as solving it using SVD.

Another alternative would be implementing something similar to Eigen’s SVD directly in gz-math, but that might also introduce some complexity.

I’m happy to follow whatever direction is preferred here.

[arjo129]

I fully agree that we should leverage Eigen’s SVD (Singular Value Decomposition) for our mathematical operations. It is a robust, industry-standard solution that makes a lot of sense for our requirements.

Before proceeding, I want to consult with the wider community to ensure there wasn't a specific historical or architectural reason why Eigen wasn't introduced to gz-math previously. If there are no blockers, I'll move forward with the integration.

[C88-YQ]

That sounds great to me! Looking forward to the outcome of the discussion.
Thanks again：）

[arjo129]

I asked the council of elders of gazebo #Gazebo PMC (Restricted posting) > Is making gz-math depend on eigen a good idea?. Eigen should be fine, only to discover we seem to redistribute eigen in this repo 🤣 .

Thanks for the quick iteration on this PR. LGTM!

[C88-YQ]

That's great! Thanks a lot for the quick review and feedback, I really appreciate it!

Once this PR is merged, I’ll mark the corresponding PR in gz-sim (currently in draft) as ready for review and continue working on the simulation-side integration.🥳

[peci1]

I saw the discussion on Zulip, but I can't comment there. I'm asking myself - if I install something called gz-math, would I expect Eigen to be brought in? Sure!

Is there an actual reason for not wanting Eigen for math?

[azeey]

This is what I commented on Zulip:

So, it's a little tricky. We do support Eigen in gz-math but it's in its own component. Currently, the core of gz-math is expected to build successfully even if you don't have Eigen installed in the system

We can have a discussion on whether we should make Eigen a dependency of gz-math core, but this is not currently the case. It will also likely not be backportable if we make the change.

Generally, I have no problem making Eigen a core dependency. @traversaro seemed to be concerned about using it in our public headers though.

[C88-YQ]

Thanks for the clarification.

Just to confirm: in my implementation Eigen is only used inside the src/SwerveDriveOdometry.cc solving the linear system via SVD, and it is not exposed in any public headers.

If this feature is expected to be backported to other versions where introducing Eigen as a core dependency is not desirable, I could also prepare an alternative implementation that does not rely on Eigen. However, that approach might be somewhat less robust compared to using Eigen‘s SVD.

[peci1]

I used Eigen’s SVD to solve the linear system that arises from the wheel velocity equations. Since the system can sometimes be overdetermined, I thought SVD would be a robust way to compute the least-squares solution. I mainly used Eigen just for solving this small linear system.

Hmm, if you just need to solve least squares, you can use pseudoinverse. It's just 3 matrix multiplications. Maybe SVD is faster in general, but this could be a way to sneak in this controller to the released distros (with potentially less efficient computation).

[scpeters]

Generally, I have no problem making Eigen a core dependency. @traversaro seemed to be concerned about using it in our public headers though.

Using Eigen in gz-math core but not in public headers seems reasonable to me. We already require the eigen component for gz-physics and gz-sim. This change would make eigen a required build dependency gz-math, so it would only be noticeable by folks building gz-math from source without gz-physics

[scpeters]

Generally, I have no problem making Eigen a core dependency. @traversaro seemed to be concerned about using it in our public headers though.

Using Eigen in gz-math core but not in public headers seems reasonable to me. We already require the eigen component for gz-physics and gz-sim. This change would make eigen a required build dependency gz-math, so it would only be noticeable by folks building gz-math from source without gz-physics

I split the Eigen dependency change out into #721

[C88-YQ]

Once #721 is merged (@scpeters thanks for opening that PR), I will rebase my branch and drop the dependency-related changes I added in this PR (e.g. the CMakeLists.txt and BUILD.bazel updates), so this PR would only keep the controller logic changes.

[C88-YQ]

@peci1 Thanks for your suggestion!

I tried implementing the computation using the native matrix / vector types already available in gz-math, but the overall implementation became somewhat cumbersome.

The linear system comes from the wheel velocity constraints of the controller. Each wheel provides one constraint relating the robot body velocity to the wheel velocity:

$v_i = \cos\theta_i v_x + \sin\theta_i v_y + (\sin\theta_i x_i - \cos\theta_i y_i)\omega$

Where:

• $v_i$ : linear velocity of wheel i
• $\theta_i$ : wheel steering angle
• $(x_i, y_i)$ : position of wheel i relative to the robot center
• $(v_x, v_y, \omega)$ : robot body velocity

By writing this constraint for the four wheels, we obtain an overdetermined linear system of the form $A v = s$, where $A$ is 4x3, $s$ is 4x1, and $v$ is 3x1. Since gz-math does not natively provide matrix types for this kind of non-square system, I had to pad the matrix with an extra zero column first, compute terms like $A_{padded}^T A_{padded}$ and $A_{padded}^T s$, and then extract the actual $A^T A$ and $A^T s$ parts to continue the solve.

In addition, my implementation separately handles the angular_v == 0 case, which leads to a 2x2 system in part of the computation. As far as I can tell, gz-math also does not provide a dedicated type for that case either, so that would require adding even more custom matrix handling code.

So overall, while a non-Eigen implementation seems possible, the computation path starts to feel somewhat awkward and verbose when expressed only with gz-math primitives. That is one reason I am hesitating on whether this is the right direction for this PR.

Because of this, I wanted to ask for advice:

• for this PR, would it be preferable that I proceed with the gz-math–based least-squares implementation (to avoid introducing Eigen and make backporting easier);
• or should I keep the current SVD-based implementation for now and not focus on backporting at this stage

As mentioned earlier, I also considered another approach which avoids solving the linear system(https://control.ros.org/humble/doc/ros2_controllers/doc/mobile_robot_kinematics.html#omnidirectional-drive-robots-using-omni-wheels:~:text=)-,Odometry,-The%20body%20twist), However, I suspect the numerical behavior might not be as good as the SVD-based method in some cases.

[C88-YQ]

Since #721 has now been merged, I’ve rebased this branch and removed the dependency-related changes that were previously included in this PR😊