Allow updating robot gravity

[AdamPettinger]

I have a UR20 mounted on another robot that can change its orientation enough to cause problems for the arm's mounting installation.

There has been some discussion on how to update the mounting orientation/gravity live (here and here), but these did not end up happening.

These changes expose the set_gravity URScript functionality through the ROS driver, allowing a user to update the gravity vector of the robot during live operation via a service. This is obviously risky if you calculate the gravity vector incorrectly.

There are corresponding changes in

• Allow updating robot gravity Universal_Robots_Client_Library#415
• Add service for setting gravity vector ros-industrial/ur_msgs#39

I just rebased these commits on main today, although we have been running these changes forked from the jazzy branch for a few months now.

---

Note

High Risk
Incorrect gravity vectors directly affect the controller’s dynamic model and can cause unsafe motion or falls; the feature is enabled by default on launch.

Overview
Adds live gravity / mounting orientation updates for UR arms by wiring URScript set_gravity through ros2_control, aimed at mobile or reorienting bases where the arm’s assumed gravity must change during operation.

A new GravityUpdateController exposes ~/set_gravity (ur_msgs/srv/SetGravity). The service accepts a gravity direction in an arbitrary TF frame, negates it to match the client library’s anti-gravity convention, rotates it into the robot base frame, and pushes it to hardware via a thread-safe box. The realtime update() loop writes x/y/z and gravity_async_success command interfaces and polls completion (same async pattern as payload/friction GPIO controllers).

The hardware interface gains a gravity GPIO in URDF/xacro, exports matching command interfaces, resets commands in initAsyncIO, and in checkAsyncIO calls ur_driver_->setGravity() when a new vector is present.

Launch and config register the plugin, add controller-manager entries, spawn gravity_update_controller active with other always-on controllers, and include it in consistent_controllers for the controller stopper.

^{Reviewed by Cursor Bugbot for commit 366559d. Bugbot is set up for automated code reviews on this repo. Configure here.}

[urfeex]

This all seems fair and well implemented. ros-industrial/ur_msgs#39 (comment) might influence this PR, however.

[urfeex]

I might have found the source of "fishyness". For all data processed by the control box, base is the correct one, not base_link.

On another topic: We would like to split up the gpio controller in the future, as it kind of became the dump place for all robot-specific functionalities. Would you mind moving this to a separate controller?

[AdamPettinger]

I might have found the source of "fishyness". For all data processed by the control box, base is the correct one, not base_link.

Ah yes, thanks! Specifically, the urcl call is expecting the "up" (away from Earth) vector in the base frame. I have left the message as gravity = down, and negated it internally to make it up in this most recent commit. Sorry about the confusion! Our test hardware was mounted so that in the neutral base position, base_link and down was the same as base and up, which made it tricky to debug

Unfortunately, this confusion existed in the URCL merge too, just in the comments here. I can go update that if you want, or you can probably just drop that specific commit

[AdamPettinger]

On another topic: We would like to split up the gpio controller in the future, as it kind of became the dump place for all robot-specific functionalities. Would you mind moving this to a separate controller?

I can give this a shot. Do you mean a gravity-specific controller, or moving some of the e.g. setPayload stuff to there as well? Or do gravity for now and others later? Based on those answers, opinion on name?

[urfeex]

Unfortunately, this confusion existed in the URCL merge too, just in the comments here. I can go update that if you want, or you can probably just drop that specific commit

Oh, yes! I missed that while reviewing. If you could update that, that would be great, then I can review it.

I can give this a shot. Do you mean a gravity-specific controller, or moving some of the e.g. setPayload stuff to there as well? Or do gravity for now and others later? Based on those answers, opinion on name?

I've been thinking about this as well last evening. I think, there are arguments for both, one UR-specific functionality controller and one controller per functionality.
Since we will not rip apart the gpio_and_status controller for released distributions we would either have to put it in there or add a new controller. Hence, I think moving it to its own controller for now is the best trade-off: We can directly backport it and if we decide to merge functionality into one controller independent of I/Os, we have to break the ROS API, anyway, then we can also merge that one in.

Name wise, what would you think of "gravity update controller"?

[AdamPettinger]

I'll split it off into a "gravity update controller". I don't know what you will do in the future, but exposing that and others (eg setPayload) as individual controllers seems reasonable in the sense that each user can tailor what controllers they want launched based on their needs. Maybe particularly relevant for setting gravity and payload as those have the potential to damage the robot if set wrong

---

It sounds like I will leave the gravity direction as is for now? The urscript docs for both the set_gravity and set_base_acceleration are generally confusing to me. For instance, set_gravity is actually expecting an acceleration, and the example calls in the set_base_acceleration don't account for gravity, so is it expecting acceleration that is in addition to gravity?

[urfeex]

Yes, I can understand that. That's why I think, we should add a more consecutive example to the controller's documentation. If you want, I can contribute that. And yes, I think we can leave the vector as it is now.

set_base_acceleration() allows to compensate for a dynamic acceleration while leaving gravity untouched. The same thing can be achieved using set_gravity(), but there you'll always have to account for gravity, as well. Which one you would use probably depends a lot on the actual application. For example, in a mobile robot it might make sense to put IMU readings into set_gravity which will account for both, gravity and dynamic acceleration. On a linear axis you might not want to touch gravity once things are setup and use set_base_acceleration() to account for the linear axis acceleration.

However, as I said, I think the scope of this PR is very well established and we can focus on that.

[urfeex]

@AdamPettinger just to be sure: I'm currently waiting for you to finish splitting this into its own controller. Or are you waiting for any input from the maintainers?

[AdamPettinger]

@AdamPettinger just to be sure: I'm currently waiting for you to finish splitting this into its own controller. Or are you waiting for any input from the maintainers?

Ah sorry, I have had a busy few weeks, I will get to this soon! Not waiting on other input

[AdamPettinger]

Ok, broke it out into its own controller and tested with our hardware. This doesn't touch the GPIO controller at all now. I did go ahead and add the gravity_update_controller to the main launch file to start by default. I also updated the directionality comments in the other repo, here: UniversalRobots/Universal_Robots_Client_Library#462

[urfeex]

This is looking good now, thanks for iterating.

I'll need some time to test this and I'll merge the service first.

[urfeex]

Tested and verified using a robot.

[cursor]

Timeout reports gravity set success

High Severity

When waitForAsyncCommand times out while the async status is still ASYNC_WAITING (2.0), setGravity sets resp->success via static_cast<bool>(success.value()). In C++, any non-zero double is true, so a pending or unverified update can return success and the “Gravity has been set successfully” status.

 

^{Reviewed by Cursor Bugbot for commit dd00fb0. Configure here.}

I missed the misaligned tag in the xacro

[cursor]

Cursor Bugbot has reviewed your changes using default effort and found 1 potential issue.

There are 3 total unresolved issues (including 2 from previous reviews).

^{Reviewed by Cursor Bugbot for commit 406f2cb. Configure here.}

[cursor]

Pending gravity command cleared

High Severity

In update, after reading a gravity vector from cmd_gravity_box_, the code always writes std::nullopt back. If another set_gravity request stores a new vector between that read and the clear, that pending command is dropped and never sent to the hardware.

 

^{Reviewed by Cursor Bugbot for commit 406f2cb. Configure here.}

[codecov]

Codecov Report

❌ Patch coverage is 0% with 105 lines in your changes missing coverage. Please review.
✅ Project coverage is 4.73%. Comparing base (1b121b7) to head (366559d).
⚠️ Report is 584 commits behind head on main.

Files with missing lines	Patch %	Lines
ur_controllers/src/gravity_update_controller.cpp	0.00%	92 Missing ⚠️
ur_robot_driver/src/hardware_interface.cpp	0.00%	13 Missing ⚠️

Additional details and impacted files

@@           Coverage Diff            @@
##            main   #1606      +/-   ##
========================================
+ Coverage   3.59%   4.73%   +1.14%     
========================================
  Files         13      35      +22     
  Lines        947    4495    +3548     
  Branches     152     530     +378     
========================================
+ Hits          34     213     +179     
- Misses       843    4277    +3434     
+ Partials      70       5      -65     

Flag	Coverage Δ
unittests	4.73% <0.00%> (+1.14%)	⬆️

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

☔ View full report in Codecov by Harness.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

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