Skip to content

Commit d854adf

Browse files
mergify[bot]mergify[bot]
authored
Allow updating robot gravity (backport #1606) (#1846)
This adds a new controller to set the robot's gravity vector.
1 parent bcde4d4 commit d854adf

11 files changed

Lines changed: 510 additions & 0 deletions

File tree

‎ur_controllers/CMakeLists.txt‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -72,6 +72,11 @@ generate_parameter_library(
7272
src/gpio_controller_parameters.yaml
7373
)
7474

75+
generate_parameter_library(
76+
gravity_update_controller_parameters
77+
src/gravity_update_controller_parameters.yaml
78+
)
79+
7580
generate_parameter_library(
7681
speed_scaling_state_broadcaster_parameters
7782
src/speed_scaling_state_broadcaster_parameters.yaml
@@ -105,6 +110,7 @@ add_library(${PROJECT_NAME} SHARED
105110
src/speed_scaling_state_broadcaster.cpp
106111
src/freedrive_mode_controller.cpp
107112
src/gpio_controller.cpp
113+
src/gravity_update_controller.cpp
108114
src/passthrough_trajectory_controller.cpp
109115
src/ur_configuration_controller.cpp)
110116

@@ -116,6 +122,7 @@ target_link_libraries(${PROJECT_NAME}
116122
force_mode_controller_parameters
117123
friction_model_controller_parameters
118124
gpio_controller_parameters
125+
gravity_update_controller_parameters
119126
speed_scaling_state_broadcaster_parameters
120127
scaled_joint_trajectory_controller_parameters
121128
freedrive_mode_controller_parameters

‎ur_controllers/controller_plugins.xml‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -44,4 +44,9 @@
4444
Controller to use the tool contact functionality of the robot.
4545
</description>
4646
</class>
47+
<class name="ur_controllers/GravityUpdateController" type="ur_controllers::GravityUpdateController" base_class_type="controller_interface::ControllerInterface">
48+
<description>
49+
Controller to set the gravity direction of the robot.
50+
</description>
51+
</class>
4752
</library>

‎ur_controllers/doc/index.rst‎

Lines changed: 95 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -461,3 +461,98 @@ The controller provides one action for enabling tool contact. For the controller
461461
.. code-block::
462462
463463
ros2 action send_goal /tool_contact_controller/detect_tool_contact ur_msgs/action/ToolContact
464+
465+
.. _ur_configuration_controller:
466+
467+
ur_controllers/URConfigurationController
468+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
469+
470+
This controller provides access to UR-specific robot configuration data. Currently, it provides a
471+
service to query the robot's software version.
472+
473+
Parameters
474+
""""""""""
475+
476+
+-------------------------+--------+---------------+---------------------------------------------------------------------------------------+
477+
| Parameter name | Type | Default value | Description |
478+
| | | | |
479+
+-------------------------+--------+---------------+---------------------------------------------------------------------------------------+
480+
| ``tf_prefix`` | string | <empty> | Urdf prefix of the corresponding arm |
481+
+-------------------------+--------+---------------+---------------------------------------------------------------------------------------+
482+
483+
Service interface / usage
484+
"""""""""""""""""""""""""
485+
486+
* ``~/get_software_version [ur_msgs/srv/GetRobotSoftwareVersion]``: Get the robot's software
487+
version. The response contains the major, minor and patch version of the robot's software, as
488+
well as the build number if available. For example, for a robot running PolyScope 5.12.1 the
489+
response would be major version 5, minor version 12 and patch version 1.
490+
491+
.. _gravity_update_controller:
492+
493+
ur_controllers/GravityUpdateController
494+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
495+
496+
This controller updates the gravity vector used by the robot controller. The robot uses this
497+
vector for gravity compensation during motion and torque control. By default, the robot assumes a
498+
gravity vector pointing straight down in its base frame (typically ``[0, 0, -9.82]`` m/s² in a
499+
standard floor-mounted setup).
500+
501+
Use this controller when the robot is mounted in a non-standard orientation, for example on a wall
502+
or ceiling, or when the base frame does not align with the physical gravity direction. The
503+
controller forwards the requested gravity vector to the robot through the URScript ``set_gravity()``
504+
function.
505+
506+
The service accepts the gravity direction in any frame that can be transformed to the robot's
507+
``base`` frame. The controller rotates the vector into the base frame and forwards it to the
508+
hardware interface.
509+
510+
.. note::
511+
512+
The service expects the **direction of gravity**, pointing towards the Earth's center. The
513+
controller negates this vector internally before sending it to the robot, as the underlying UR
514+
client library expects an anti-gravity vector (pointing away from the Earth's center).
515+
516+
Parameters
517+
""""""""""
518+
519+
+-------------------------------------+--------+---------------+---------------------------------------------------------------------+
520+
| Parameter name | Type | Default value | Description |
521+
| | | | |
522+
+-------------------------------------+--------+---------------+---------------------------------------------------------------------+
523+
| ``tf_prefix`` | string | <empty> | Urdf prefix of the corresponding arm |
524+
+-------------------------------------+--------+---------------+---------------------------------------------------------------------+
525+
| ``check_io_successfull_retries`` | int | 10 | Amount of retries for checking if setting gravity was successful |
526+
+-------------------------------------+--------+---------------+---------------------------------------------------------------------+
527+
528+
Service interface / usage
529+
"""""""""""""""""""""""""
530+
531+
The controller provides a service for setting the gravity vector. To use this service, the
532+
controller has to be in ``active`` state.
533+
534+
* ``~/set_gravity [ur_msgs/srv/SetGravity]``: Set the gravity direction experienced by the robot.
535+
536+
The request contains a ``geometry_msgs/Vector3Stamped`` named ``gravity``. The vector specifies
537+
the direction of gravity (towards the Earth's center) and ``header.frame_id`` specifies the frame
538+
in which the vector is expressed. Any frame that can be transformed to the robot's ``base`` frame
539+
can be used.
540+
541+
Example for a standard floor-mounted robot (gravity pointing down in the ``base`` frame):
542+
543+
.. code-block:: console
544+
545+
ros2 service call /gravity_update_controller/set_gravity ur_msgs/srv/SetGravity \
546+
"{gravity: {header: {frame_id: 'base'}, vector: {x: 0.0, y: 0.0, z: -9.82}}}"
547+
548+
Example for a ceiling-mounted robot (gravity pointing up in the ``base`` frame):
549+
550+
.. code-block:: console
551+
552+
ros2 service call /gravity_update_controller/set_gravity ur_msgs/srv/SetGravity \
553+
"{gravity: {header: {frame_id: 'base'}, vector: {x: 0.0, y: 0.0, z: 9.82}}}"
554+
555+
.. note::
556+
557+
When using the mocked hardware interface, the service may report failure even though the command
558+
was accepted, because the mock does not emulate the asynchronous success feedback from the robot.

‎ur_controllers/include/ur_controllers/gravity_update_controller.hpp‎

Lines changed: 123 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,123 @@
1+
// Copyright (c) 2026 PickNik LLC
2+
//
3+
// Redistribution and use in source and binary forms, with or without
4+
// modification, are permitted provided that the following conditions are met:
5+
//
6+
// * Redistributions of source code must retain the above copyright
7+
// notice, this list of conditions and the following disclaimer.
8+
//
9+
// * Redistributions in binary form must reproduce the above copyright
10+
// notice, this list of conditions and the following disclaimer in the
11+
// documentation and/or other materials provided with the distribution.
12+
//
13+
// * Neither the name of the {copyright_holder} nor the names of its
14+
// contributors may be used to endorse or promote products derived from
15+
// this software without specific prior written permission.
16+
//
17+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18+
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19+
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20+
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21+
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22+
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23+
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24+
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25+
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26+
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27+
// POSSIBILITY OF SUCH DAMAGE.
28+
29+
//----------------------------------------------------------------------
30+
/*!\file
31+
*
32+
* \brief Controller for updating the robot's gravity vector via ros2_control.
33+
*
34+
* Exposes the ~/set_gravity service (ur_msgs/srv/SetGravity). Incoming gravity directions are
35+
* transformed into the robot base frame and written to the gravity command interfaces.
36+
*
37+
* \author Adam Pettinger adam.l.pettinger@gmail.com
38+
* \date 2026-03-13
39+
*
40+
*/
41+
//----------------------------------------------------------------------
42+
43+
#ifndef UR_CONTROLLERS__GRAVITY_UPDATE_CONTROLLER_HPP_
44+
#define UR_CONTROLLERS__GRAVITY_UPDATE_CONTROLLER_HPP_
45+
46+
#include <memory>
47+
#include <string>
48+
#include <unordered_map>
49+
#include <vector>
50+
51+
#include "controller_interface/controller_interface.hpp"
52+
#include "ur_msgs/srv/set_gravity.hpp"
53+
#include "rclcpp/time.hpp"
54+
#include "rclcpp/duration.hpp"
55+
#include "realtime_tools/realtime_thread_safe_box.hpp"
56+
#include "tf2/LinearMath/Quaternion.hpp"
57+
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
58+
#include "tf2_ros/buffer.hpp"
59+
#include "tf2_ros/transform_listener.hpp"
60+
#include "ur_controllers/gravity_update_controller_parameters.hpp"
61+
62+
namespace ur_controllers
63+
{
64+
enum CommandInterfaces
65+
{
66+
GRAVITY_X,
67+
GRAVITY_Y,
68+
GRAVITY_Z,
69+
GRAVITY_ASYNC_SUCCESS,
70+
};
71+
72+
enum StateInterfaces
73+
{
74+
INITIALIZED_FLAG = 69,
75+
};
76+
77+
class GravityUpdateController : public controller_interface::ControllerInterface
78+
{
79+
public:
80+
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
81+
82+
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
83+
84+
controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
85+
86+
CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
87+
88+
CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
89+
90+
CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
91+
92+
CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override;
93+
94+
CallbackReturn on_init() override;
95+
96+
private:
97+
void setGravity(const ur_msgs::srv::SetGravity::Request::SharedPtr req,
98+
ur_msgs::srv::SetGravity::Response::SharedPtr resp);
99+
100+
protected:
101+
rclcpp::Service<ur_msgs::srv::SetGravity>::SharedPtr set_gravity_srv_;
102+
103+
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
104+
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
105+
106+
// Parameters from ROS for gravity_update_controller
107+
std::shared_ptr<gravity_update_controller::ParamListener> param_listener_;
108+
gravity_update_controller::Params params_;
109+
110+
static constexpr double ASYNC_WAITING = 2.0;
111+
112+
realtime_tools::RealtimeThreadSafeBox<std::optional<tf2::Vector3>> cmd_gravity_box_;
113+
realtime_tools::RealtimeThreadSafeBox<std::optional<double>> async_success_box_;
114+
115+
/**
116+
* @brief wait until a command interface isn't in state ASYNC_WAITING anymore or until the parameter maximum_retries
117+
* have been reached
118+
*/
119+
bool waitForAsyncCommand(std::function<double(void)> get_value);
120+
};
121+
} // namespace ur_controllers
122+
123+
#endif // UR_CONTROLLERS__GRAVITY_UPDATE_CONTROLLER_HPP_

0 commit comments

Comments
 (0)