Add base twist plugin for commanding the mobile base by saikishor · Pull Request #254 · ros-controls/mujoco_ros2_control

saikishor · 2026-07-27T14:53:46Z

Description

This PR adds a new plugin that can control the base of the robot by commanding directly the floating joint it is connected to directly; it resembles the ROS planar move plugin that Gazebo offers. This is very reliable as the users doesn't have to spend time modeling their wheels and contact friction etc, and this basically helps to fast prototype

Is this user-facing behavior change?

NO

Did you use Generative AI?

Claude

Additional Information

Grabacion.de.pantalla.desde.27-07-26.15_26_29.1.webm

saikishor · 2026-07-27T15:09:29Z

I'm not sure of the approach I'm choosing here, right now what I do is the mjData that we pass to the update method of the plugin, I'm setting them to NaNs, and then if the plugin sets them to a value, then that's what I'm using to override the original values in the mjStep

codecov-commenter · 2026-07-27T15:18:58Z

Codecov Report

❌ Patch coverage is 88.67925% with 24 lines in your changes missing coverage. Please review.
✅ Project coverage is 80.55%. Comparing base (66c173e) to head (a74d696).
⚠️ Report is 1 commits behind head on main.

Files with missing lines	Patch %	Lines
..._ros2_control_plugins/src/base_velocity_plugin.cpp	79.16%	8 Missing and 7 partials ⚠️
...control_plugins/test/test_base_velocity_plugin.cpp	93.12%	3 Missing and 6 partials ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main     #254      +/-   ##
==========================================
+ Coverage   79.96%   80.55%   +0.59%     
==========================================
  Files          27       30       +3     
  Lines        4642     4907     +265     
  Branches      518      538      +20     
==========================================
+ Hits         3712     3953     +241     
- Misses        657      668      +11     
- Partials      273      286      +13

Flag	Coverage Δ
unittests	`80.55% <88.67%> (+0.59%)`	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
.../include/mujoco_ros2_control/mujoco_simulation.hpp	`100.00% <ø> (ø)`
mujoco_ros2_control/src/mujoco_simulation.cpp	`66.46% <100.00%> (+0.30%)`	⬆️
...ujoco_ros2_control/src/mujoco_system_interface.cpp	`60.40% <100.00%> (+0.04%)`	⬆️
...ntrol_plugins/mujoco_ros2_control_plugins_base.hpp	`100.00% <ø> (ø)`
..._ros2_control_plugins/src/base_velocity_plugin.hpp	`100.00% <100.00%> (ø)`
...control_plugins/test/test_base_velocity_plugin.cpp	`93.12% <93.12%> (ø)`
..._ros2_control_plugins/src/base_velocity_plugin.cpp	`79.16% <79.16%> (ø)`

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

eholum-nasa

Funnily enough we've been considering this exact problem for our ridgeback.

We do whole body motion planning with Phoebe which requires "joints" to attach the base to world (linear x, y, and a rotational yaw), so those joints existed anyway. To make the base into a magic carpet we just make those joints real and exclude contacts between the wheels, base, and floor. From there it's just a forward command controller fed from a mecanum drive controller -> joint command republisher. The full changes are in:

NASA-JSC-Robotics/phoebe_bridgeback#49

Where the magic carpet commands are just handled here.

Would something like that support your use case? We've found it more useful for handling the sim2realness of an actual mobile base

I feel like it is less invasive than adding another piece of overridable data for the plugin.

saikishor · 2026-07-27T16:16:14Z

We do whole body motion planning with Phoebe which requires "joints" to attach the base to world (linear x, y, and a rotational yaw), so those joints existed anyway. To make the base into a magic carpet we just make those joints real and exclude contacts between the wheels, base, and floor. From there it's just a forward command controller fed from a mecanum drive controller -> joint command republisher. The full changes are in:

NASA-JSC-Robotics/phoebe_bridgeback#49

Where the magic carpet commands are just handled here.

Would something like that support your use case? We've found it more useful for handling the sim2realness of an actual mobile base

I feel like it is less invasive than adding another piece of overridable data for the plugin.

This might work too. However, I think there is quite some difference in the approach, for a beginner user modifying the XML to add new joints to make it work, instead with the proposed approach, adding a plugin to a yaml already gives him the flexibility to start and run without modifying much his simulation setup. I think it is a big win in this scenario. Moreover, Gazebo also offered similar kind of plugin in the past : https://github.com/ros-simulation/gazebo_ros_pkgs/blob/noetic-devel/gazebo_plugins/src/gazebo_ros_planar_move.cpp. I'm a bit aligned with the proposed approach for the simplicity on the user end.

scastro-nasa · 2026-07-27T20:08:24Z

I agree with @saikishor that the python republishing thing is pretty complicated for new users, but also with @eholum-nasa that needing to modify mujoco_simulation.cpp/.hpp is kind of a code smell.

Is there a solution where we don't touch the mujoco_ros2_control source and instead create an actual C++ ROS 2 controller that can publish the base in this ideal form? Then for newcomers it's just a matter of activating this controller and publishing /cmd_vel to it.

saikishor · 2026-07-27T20:58:21Z

I agree with @saikishor that the python republishing thing is pretty complicated for new users, but also with @eholum-nasa that needing to modify mujoco_simulation.cpp/.hpp is kind of a code smell.

Is there a solution where we don't touch the mujoco_ros2_control source and instead create an actual C++ ROS 2 controller that can publish the base in this ideal form? Then for newcomers it's just a matter of activating this controller and publishing /cmd_vel to it.

As of now, there is no easier way as the floating base joints are not exposed as controllable interfaces through ROS Controls interfaces. Even though we expose, we don't have a n out-of-the-box controller that does that work for you. I can rethink this part.

I agree that touching the mujoco_simulation file is not ideal, as per every implementation we come across limitations on that end.

eholum-nasa · 2026-07-29T17:31:26Z

I do think our solution to use a "magic carpet controller" is a reasonable one,

https://github.com/NASA-JSC-Robotics/phoebe_bridgeback/pull/49/changes

I don't think those changes were too significant, a lot of them were just to handle that we have a crazy number of deployment modes to support for Phoebe. That said, the controller + the linear rails is a very straightforward way to end up with perfect odometry if that's what you're looking for? And it matches our existing control setup with the mecanum drive so it's still somewhat seamless from nav2's perspective?

saikishor · 2026-08-04T19:49:58Z

I do think our solution to use a "magic carpet controller" is a reasonable one,

https://github.com/NASA-JSC-Robotics/phoebe_bridgeback/pull/49/changes

I don't think those changes were too significant, a lot of them were just to handle that we have a crazy number of deployment modes to support for Phoebe. That said, the controller + the linear rails is a very straightforward way to end up with perfect odometry if that's what you're looking for? And it matches our existing control setup with the mecanum drive so it's still somewhat seamless from nav2's perspective?

I don't know If this is the way. I don't think it is right to impose everyone to set these linear rails just to be able to be able to track their base without slipping. I think it is better to leave it simple like a plugin no?. Even with this plugin, we are publishing to the same topic /cmd_vel, so everything should be working as it is supposed to work with the proposed approach in this PR right?

eholum-nasa · 2026-08-04T21:34:07Z

+   * @note data->qvel is NaN-filled before every plugin's update() runs this cycle. Write a finite value into any
+   * entry to request a hard velocity override on that DOF -- it bypasses the normal mass/contact dynamics for that
+   * DOF this step, overwriting mj_data_->qvel directly right before the next mj_step. Leave an entry NaN to keep its
+   * normal physics-driven value untouched. This is the only channel a plugin has to influence qvel: unlike
+   * ctrl/qfrc_applied/xfrc_applied, data->qvel here cannot be read for the body's actual velocity, since it is not
+   * restored to a real value until after every plugin's update() has run this cycle.


This feels like a large can of worms to open for the sake of free-flying magic carpets, especially when the engine already supports this through other approaches (like the rails). That said, I think we can all agree that MuJoCo's wheel/contact model makes this genuinely hard, and I don't disagree with providing a mechanism to make it easier.

If we're going to open this can of worms, I think it's worth revisiting an idea we've bumped up against a few times - expanding the plugin interface with a pre-step hook that gives plugins direct access to mj_data_ immediately before mj_step in the physics loop.

Something like:

virtual void update_pre_step(const mjModel* model, mjData* data) { (void)model; (void)data; }

If we threw that in and put the CB into the PhysicsLoop then plugins really would have full god-tier access to the sim. We could remove both the qvel_override_staged_ and the xfrc_plugin_desired_ staging mechanisms. This would give plugins way more power, but IMO that would be the "right" thing to do rather than to continue digging very deep hooks deep into simulate. I am not the biggest fan of the use-case-specific staging vectors that we have to handle... so maybe that would make things easier for the next time a plugin needs access to a new field?

Thoughts?

I think I would be down to move forward with this for now if we commit ourselves to a refactor like what I've proposed above rather than add any more one-offs (rule of three).

I agree with you. I'm not proud of this implementation. Moreover, if any plugin uses qvel for their use case, then they will receive NaNs instead of a valid qvel. This is something I dislike in this approach. I also agree with the limitations and how much this is causing the design to keep editing the primary system interface.

ndunkelb-nasa

Just a couple of comments to check some implementation details!

ndunkelb-nasa · 2026-08-04T22:31:08Z

+     - ``bool``
+     - ``false``
+     - Subscribe to ``geometry_msgs/TwistStamped`` instead of ``geometry_msgs/Twist`` (e.g. for
+       Nav2, which publishes stamped twists by default in some configurations).


I would add to this that if you set this to true, the header is thrown away, bc sometimes the assumption, especially with cmd_timeout might be that the stamp needs to be close enough to real time to work.

ndunkelb-nasa · 2026-08-04T22:33:43Z

+  if (body_id_ < 0 || qvel_adr_ < 0)
+  {
+    return;
+  }


I think you took care of these failure cases in init(), right?

ndunkelb-nasa · 2026-08-04T22:38:36Z

+  // Step 2 - a stale (or never-received) command is treated as a zero-velocity
+  // command, i.e. the base is commanded to stop rather than coast on the last override.
+  double vx_cmd = 0.0, vy_cmd = 0.0, wz_cmd = 0.0;
+  if (cached_cmd_.valid)


Seems like this is never not valid? Just set to true when initialized, then not ever set to false...

eholum-nasa

Definitely does what it promises! Side note that I appreciate all the warnings about breaking physics.

Screen.Recording.2026-08-05.at.9.51.34.AM.mov

Just a minor nit and the philosophical design question, but otherwise I think this is in a good state.

eholum-nasa · 2026-08-05T13:33:23Z

+wheels and ground have zero friction, which is now largely cosmetic -- propulsion no longer goes
+through wheel-ground contact at all, so it does not depend on friction either way.
+
+.. warning::


Nice, this explains the danger well!

eholum-nasa · 2026-08-05T13:55:49Z

+  }
+  else
+  {
+    twist_sub_ = node_->create_subscription<geometry_msgs::msg::Twist>(cmd_vel_topic, rclcpp::SystemDefaultsQoS(),


My ignorance on ros2_controllers, but are there controllers for wheeled bases that ingest twists and not stamped twists?

Nav2 over the last few ROS distros has attempted to migrate from Twist to TwistStamped:
https://discourse.openrobotics.org/t/nav2-support-for-twiststamped-available-rolling-jazzy/35995

Note the post also mentions Gazebo and ros2_control, so ... probably stamped is the way to go since it's a superset of twist?

Yes, it is better to go with TwistStamped. For humble versions, Nav2 was still with Twist IIRC. Having an option to toggle helps. Maybe I can change the default

eholum-nasa · 2026-08-05T14:11:35Z

+    double vy{ 0.0 };
+    double wz{ 0.0 };
+    rclcpp::Time time{ 0, 0, RCL_ROS_TIME };
+    bool valid{ false };


I don't think valid is necessary? Could remove it and just let the timeout check either catch it, but everything defaults to 0 anyway.

saikishor · 2026-08-05T15:56:45Z

I'll respond to you guys tonight once I reach home. Thank you for the review

saikishor added 13 commits July 27, 2026 17:04

Add base velocity plugin to control floating base

5e9c39f

Add corresponding tests

818f108

Update documentation

014e247

Add a demo demonstrating the mobile base control plugin

bdd3ada

Simplify the logic

a50b529

Simplify comments

15f7ab7

Fix the parameter namespacing

d3c7ee5

Add qvel_override_staged_ to override qvel from the plugins

e1ccb3b

Add the logic in system interface

b9a930d

Add corresponding changes on the plugin side

833ebcd

Update tests

b4ab525

Update documentation

f1fa5c5

Update demo

26cbb60

saikishor force-pushed the add/base_twist_plugin branch from 62d5903 to 26cbb60 Compare July 27, 2026 15:06

Apply pre-commit changes

a74d696

saikishor requested review from eholum-nasa, ndunkelb-nasa and scastro-nasa and removed request for eholum-nasa and ndunkelb-nasa July 27, 2026 15:19

eholum-nasa reviewed Jul 27, 2026

View reviewed changes

Comment thread mujoco_ros2_control/src/mujoco_simulation.cpp

eholum-nasa reviewed Aug 4, 2026

View reviewed changes

ndunkelb-nasa reviewed Aug 5, 2026

View reviewed changes

eholum-nasa reviewed Aug 5, 2026

View reviewed changes

Address MR comments

83a5814

saikishor mentioned this pull request Aug 5, 2026

Simplify mujoco_ros2_control_plugins integration #262

Open

6 tasks

Uh oh!

Conversation

saikishor commented Jul 27, 2026

Description

Is this user-facing behavior change?

Did you use Generative AI?

Additional Information

Uh oh!

saikishor commented Jul 27, 2026

Uh oh!

codecov-commenter commented Jul 27, 2026

Codecov Report

Uh oh!

eholum-nasa left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

saikishor commented Jul 27, 2026

Uh oh!

scastro-nasa commented Jul 27, 2026

Uh oh!

saikishor commented Jul 27, 2026

Uh oh!

eholum-nasa commented Jul 29, 2026

Uh oh!

saikishor commented Aug 4, 2026

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

ndunkelb-nasa left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

eholum-nasa left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

saikishor commented Aug 5, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

5 participants