Skip to content

Allow setting payload inertia matrix via set_payload service backwards-compatible #1811

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
srvald wants to merge 10 commits into
base: main
Choose a base branch
from
+307 −9
Open

Allow setting payload inertia matrix via set_payload service backwards-compatible #1811

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
68242d6
feat: extend set_payload service with inertia matrix and transition_time
srvald Jun 5, 2026
b5c8486
feat: extend set_payload service with inertia matrix and transition_t…
srvald Jun 8, 2026
d957685
fix: prevent race condition in async payload execution
srvald Jun 8, 2026
66ba497
style: fix formatting issues
srvald Jun 8, 2026
19806c1
style: fix missing formatting issues
srvald Jun 8, 2026
925f87b
feat: Use builtin_interfaces/Duration for transition_time
srvald Jun 9, 2026
8cb48f3
Merge branch 'main' into set_target_payload_compatible
srvald Jul 3, 2026
25a4c0c
fix: PAYLOAD_STATE_TRANSITION_TIME removed
srvald Jul 3, 2026
10e7fba
fix: unused variable removed
srvald Jul 3, 2026
3731249
fix: No executable and format adjusted
srvald Jul 3, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 15 additions & 1 deletion ur_controllers/include/ur_controllers/gpio_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,13 @@ enum CommandInterfaces
HAND_BACK_CONTROL_CMD = 33,
HAND_BACK_CONTROL_ASYNC_SUCCESS = 34,
ANALOG_OUTPUTS_DOMAIN = 35,
PAYLOAD_INERTIA_IXX = 36,
PAYLOAD_INERTIA_IYY = 37,
PAYLOAD_INERTIA_IZZ = 38,
PAYLOAD_INERTIA_IXY = 39,
PAYLOAD_INERTIA_IXZ = 40,
PAYLOAD_INERTIA_IYZ = 41,
PAYLOAD_TRANSITION_TIME = 42,
};

enum StateInterfaces
Expand All @@ -107,6 +114,12 @@ enum StateInterfaces
PAYLOAD_STATE_COG_X = 72,
PAYLOAD_STATE_COG_Y = 73,
PAYLOAD_STATE_COG_Z = 74,
PAYLOAD_STATE_INERTIA_IXX = 75,
PAYLOAD_STATE_INERTIA_IYY = 76,
PAYLOAD_STATE_INERTIA_IZZ = 77,
PAYLOAD_STATE_INERTIA_IXY = 78,
PAYLOAD_STATE_INERTIA_IXZ = 79,
PAYLOAD_STATE_INERTIA_IYZ = 80,
};

class GPIOController : public controller_interface::ControllerInterface
Expand Down Expand Up @@ -210,7 +223,8 @@ class GPIOController : public controller_interface::ControllerInterface
*/
bool waitForAsyncCommand(std::function<double(void)> get_value);

bool waitForPayloadRtdeMatch(double mass, double cx, double cy, double cz);
bool waitForPayloadRtdeMatch(double mass, double cx, double cy, double cz, double ixx, double iyy, double izz,
double ixy, double ixz, double iyz, double transition_time);
};
} // namespace ur_controllers

Expand Down
50 changes: 45 additions & 5 deletions ur_controllers/src/gpio_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -135,6 +135,14 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c

config.names.emplace_back(tf_prefix + "gpio/analog_output_domain_cmd");

config.names.emplace_back(tf_prefix + "payload/inertia.ixx");
config.names.emplace_back(tf_prefix + "payload/inertia.iyy");
config.names.emplace_back(tf_prefix + "payload/inertia.izz");
config.names.emplace_back(tf_prefix + "payload/inertia.ixy");
config.names.emplace_back(tf_prefix + "payload/inertia.ixz");
config.names.emplace_back(tf_prefix + "payload/inertia.iyz");
config.names.emplace_back(tf_prefix + "payload/transition_time");

return config;
}

Expand Down Expand Up @@ -200,6 +208,12 @@ controller_interface::InterfaceConfiguration ur_controllers::GPIOController::sta
config.names.emplace_back(tf_prefix + "payload/cog.x");
config.names.emplace_back(tf_prefix + "payload/cog.y");
config.names.emplace_back(tf_prefix + "payload/cog.z");
config.names.emplace_back(tf_prefix + "payload/inertia.ixx");
config.names.emplace_back(tf_prefix + "payload/inertia.iyy");
config.names.emplace_back(tf_prefix + "payload/inertia.izz");
config.names.emplace_back(tf_prefix + "payload/inertia.ixy");
config.names.emplace_back(tf_prefix + "payload/inertia.ixz");
config.names.emplace_back(tf_prefix + "payload/inertia.iyz");

return config;
}
Expand Down Expand Up @@ -618,6 +632,15 @@ bool GPIOController::setPayload(const ur_msgs::srv::SetPayload::Request::SharedP
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_X].set_value(req->center_of_gravity.x);
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_Y].set_value(req->center_of_gravity.y);
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_Z].set_value(req->center_of_gravity.z);
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IXX].set_value(req->ixx);
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IYY].set_value(req->iyy);
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IZZ].set_value(req->izz);
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IXY].set_value(req->ixy);
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IXZ].set_value(req->ixz);
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IYZ].set_value(req->iyz);
double transition_time_seconds =
static_cast<double>(req->transition_time.sec) + (static_cast<double>(req->transition_time.nanosec) * 1e-9);
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_TRANSITION_TIME].set_value(transition_time_seconds);

if (!waitForAsyncCommand([&]() {
return command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING);
Expand All @@ -636,8 +659,10 @@ bool GPIOController::setPayload(const ur_msgs::srv::SetPayload::Request::SharedP

if (params_.verify_payload_on_set) {
if (!waitForPayloadRtdeMatch(static_cast<double>(req->mass), req->center_of_gravity.x, req->center_of_gravity.y,
req->center_of_gravity.z)) {
RCLCPP_WARN(get_node()->get_logger(), "setPayload reported success but RTDE payload / payload_cog do not match "
req->center_of_gravity.z, req->ixx, req->iyy, req->izz, req->ixy, req->ixz, req->iyz,
transition_time_seconds)) {
RCLCPP_WARN(get_node()->get_logger(), "setPayload reported success but RTDE payload / payload_cog / "
"payload_inertia do not match "
"the "
"request yet. (This might "
"happen when using the mocked interface.)");
Expand Down Expand Up @@ -706,20 +731,35 @@ bool GPIOController::waitForAsyncCommand(std::function<double(void)> get_value)
return true;
}

bool GPIOController::waitForPayloadRtdeMatch(double mass, double cx, double cy, double cz)
bool GPIOController::waitForPayloadRtdeMatch(double mass, double cx, double cy, double cz, double ixx, double iyy,
double izz, double ixy, double ixz, double iyz, double transition_time)
{
constexpr double tol_mass = 1e-3;
constexpr double tol_cog = 1e-4;
constexpr double tol_inertia = 1e-4;
const auto maximum_retries = params_.check_io_successfull_retries;

if (transition_time > 0.0) {
std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(transition_time * 1000)));
}

for (int retries = 0; retries <= maximum_retries; ++retries) {
const auto m = state_interfaces_[StateInterfaces::PAYLOAD_STATE_MASS].get_optional();
const auto sx = state_interfaces_[StateInterfaces::PAYLOAD_STATE_COG_X].get_optional();
const auto sy = state_interfaces_[StateInterfaces::PAYLOAD_STATE_COG_Y].get_optional();
const auto sz = state_interfaces_[StateInterfaces::PAYLOAD_STATE_COG_Z].get_optional();
if (m && sx && sy && sz) {
const auto sixx = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IXX].get_optional();
const auto siyy = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IYY].get_optional();
const auto sizz = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IZZ].get_optional();
const auto sixy = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IXY].get_optional();
const auto sixz = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IXZ].get_optional();
const auto siyz = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IYZ].get_optional();
if (m && sx && sy && sz && sixx && siyy && sizz && sixy && sixz && siyz) {
if (std::abs(*m - mass) <= tol_mass && std::abs(*sx - cx) <= tol_cog && std::abs(*sy - cy) <= tol_cog &&
std::abs(*sz - cz) <= tol_cog) {
std::abs(*sz - cz) <= tol_cog && std::abs(*sixx - ixx) <= tol_inertia &&
std::abs(*siyy - iyy) <= tol_inertia && std::abs(*sizz - izz) <= tol_inertia &&
std::abs(*sixy - ixy) <= tol_inertia && std::abs(*sixz - ixz) <= tol_inertia &&
std::abs(*siyz - iyz) <= tol_inertia) {
return true;
}
}
Expand Down
4 changes: 4 additions & 0 deletions ur_robot_driver/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -286,6 +286,10 @@ if(BUILD_TESTING)
TIMEOUT
800
)
add_launch_test(test/integration_test_payload.py
TIMEOUT
800
)
add_launch_test(test/integration_test_force_mode.py
TIMEOUT
800
Expand Down
3 changes: 3 additions & 0 deletions ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -284,10 +284,13 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface

// Payload stuff
urcl::vector3d_t payload_center_of_gravity_;
urcl::vector6d_t payload_inertia_;
double payload_mass_;
double payload_transition_time_;
double payload_async_success_;
double rtde_payload_mass_ = 0.0;
urcl::vector3d_t rtde_payload_cog_{ 0.0, 0.0, 0.0 };
urcl::vector6d_t rtde_payload_inertia_{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };

// Friction model parameters
urcl::vector6d_t friction_model_viscous_;
Expand Down
1 change: 1 addition & 0 deletions ur_robot_driver/resources/rtde_output_recipe.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,3 +29,4 @@ actual_current
tcp_offset
payload
payload_cog
payload_inertia
40 changes: 37 additions & 3 deletions ur_robot_driver/src/hardware_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -415,7 +415,18 @@ std::vector<hardware_interface::StateInterface> URPositionHardwareInterface::exp
hardware_interface::StateInterface(tf_prefix + "payload", "cog.y", &rtde_payload_cog_[1]));
state_interfaces.emplace_back(
hardware_interface::StateInterface(tf_prefix + "payload", "cog.z", &rtde_payload_cog_[2]));

state_interfaces.emplace_back(
hardware_interface::StateInterface(tf_prefix + "payload", "inertia.ixx", &rtde_payload_inertia_[0]));
state_interfaces.emplace_back(
hardware_interface::StateInterface(tf_prefix + "payload", "inertia.iyy", &rtde_payload_inertia_[1]));
state_interfaces.emplace_back(
hardware_interface::StateInterface(tf_prefix + "payload", "inertia.izz", &rtde_payload_inertia_[2]));
state_interfaces.emplace_back(
hardware_interface::StateInterface(tf_prefix + "payload", "inertia.ixy", &rtde_payload_inertia_[3]));
state_interfaces.emplace_back(
hardware_interface::StateInterface(tf_prefix + "payload", "inertia.ixz", &rtde_payload_inertia_[4]));
state_interfaces.emplace_back(
hardware_interface::StateInterface(tf_prefix + "payload", "inertia.iyz", &rtde_payload_inertia_[5]));
// Motion primitives stuff
state_interfaces.emplace_back(hardware_interface::StateInterface(tf_prefix + HW_IF_MOTION_PRIMITIVES,
"execution_status", &hw_moprim_states_[0]));
Expand Down Expand Up @@ -478,6 +489,20 @@ std::vector<hardware_interface::CommandInterface> URPositionHardwareInterface::e
hardware_interface::CommandInterface(tf_prefix + "payload", "cog.y", &payload_center_of_gravity_[1]));
command_interfaces.emplace_back(
hardware_interface::CommandInterface(tf_prefix + "payload", "cog.z", &payload_center_of_gravity_[2]));
command_interfaces.emplace_back(
hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.ixx", &payload_inertia_[0]));
command_interfaces.emplace_back(
hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.iyy", &payload_inertia_[1]));
command_interfaces.emplace_back(
hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.izz", &payload_inertia_[2]));
command_interfaces.emplace_back(
hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.ixy", &payload_inertia_[3]));
command_interfaces.emplace_back(
hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.ixz", &payload_inertia_[4]));
command_interfaces.emplace_back(
hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.iyz", &payload_inertia_[5]));
command_interfaces.emplace_back(
hardware_interface::CommandInterface(tf_prefix + "payload", "transition_time", &payload_transition_time_));
command_interfaces.emplace_back(
hardware_interface::CommandInterface(tf_prefix + "payload", "payload_async_success", &payload_async_success_));

Expand Down Expand Up @@ -999,6 +1024,7 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp::
readData(data_package_buffer_, "tcp_offset", tcp_offset_);
readData(data_package_buffer_, "payload", rtde_payload_mass_);
readData(data_package_buffer_, "payload_cog", rtde_payload_cog_);
readData(data_package_buffer_, "payload_inertia", rtde_payload_inertia_);

// required transforms
extractToolPose();
Expand Down Expand Up @@ -1138,6 +1164,8 @@ void URPositionHardwareInterface::initAsyncIO()

payload_mass_ = NO_NEW_CMD_;
payload_center_of_gravity_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
payload_inertia_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
payload_transition_time_ = NO_NEW_CMD_;

gravity_vector_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
friction_model_viscous_.fill(NO_NEW_CMD_);
Expand Down Expand Up @@ -1205,10 +1233,16 @@ void URPositionHardwareInterface::checkAsyncIO()

if (!std::isnan(payload_mass_) && !std::isnan(payload_center_of_gravity_[0]) &&
!std::isnan(payload_center_of_gravity_[1]) && !std::isnan(payload_center_of_gravity_[2]) &&
ur_driver_ != nullptr) {
payload_async_success_ = ur_driver_->setPayload(payload_mass_, payload_center_of_gravity_);
!std::isnan(payload_inertia_[0]) && !std::isnan(payload_inertia_[1]) && !std::isnan(payload_inertia_[2]) &&
!std::isnan(payload_inertia_[3]) && !std::isnan(payload_inertia_[4]) && !std::isnan(payload_inertia_[5]) &&
!std::isnan(payload_transition_time_) && ur_driver_ != nullptr) {
payload_async_success_ = ur_driver_->setTargetPayload(payload_mass_, payload_center_of_gravity_, payload_inertia_,
payload_transition_time_);

payload_mass_ = NO_NEW_CMD_;
payload_center_of_gravity_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
payload_inertia_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
payload_transition_time_ = NO_NEW_CMD_;
Comment thread
cursor[bot] marked this conversation as resolved.
}

if (!std::isnan(gravity_vector_[0]) && !std::isnan(gravity_vector_[1]) && !std::isnan(gravity_vector_[2]) &&
Expand Down
Loading
Loading