Skip to content

Commit 7fb09e2

Browse files
mergify[bot]mergify[bot]
authored
Allow setting payload inertia matrix via set_payload service (backport #1811) (#1878)
Extend set_payload service with inertia matrix and transition_time
1 parent bade17f commit 7fb09e2

7 files changed

Lines changed: 176 additions & 13 deletions

File tree

‎ur_controllers/include/ur_controllers/gpio_controller.hpp‎

Lines changed: 15 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -82,6 +82,13 @@ enum CommandInterfaces
8282
HAND_BACK_CONTROL_CMD = 33,
8383
HAND_BACK_CONTROL_ASYNC_SUCCESS = 34,
8484
ANALOG_OUTPUTS_DOMAIN = 35,
85+
PAYLOAD_INERTIA_IXX = 36,
86+
PAYLOAD_INERTIA_IYY = 37,
87+
PAYLOAD_INERTIA_IZZ = 38,
88+
PAYLOAD_INERTIA_IXY = 39,
89+
PAYLOAD_INERTIA_IXZ = 40,
90+
PAYLOAD_INERTIA_IYZ = 41,
91+
PAYLOAD_TRANSITION_TIME = 42,
8592
};
8693

8794
enum StateInterfaces
@@ -107,6 +114,12 @@ enum StateInterfaces
107114
PAYLOAD_STATE_COG_X = 72,
108115
PAYLOAD_STATE_COG_Y = 73,
109116
PAYLOAD_STATE_COG_Z = 74,
117+
PAYLOAD_STATE_INERTIA_IXX = 75,
118+
PAYLOAD_STATE_INERTIA_IYY = 76,
119+
PAYLOAD_STATE_INERTIA_IZZ = 77,
120+
PAYLOAD_STATE_INERTIA_IXY = 78,
121+
PAYLOAD_STATE_INERTIA_IXZ = 79,
122+
PAYLOAD_STATE_INERTIA_IYZ = 80,
110123
};
111124

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

213-
bool waitForPayloadRtdeMatch(double mass, double cx, double cy, double cz);
226+
bool waitForPayloadRtdeMatch(double mass, double cx, double cy, double cz, double ixx, double iyy, double izz,
227+
double ixy, double ixz, double iyz, double transition_time);
214228
};
215229
} // namespace ur_controllers
216230

‎ur_controllers/src/gpio_controller.cpp‎

Lines changed: 45 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -135,6 +135,14 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c
135135

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

138+
config.names.emplace_back(tf_prefix + "payload/inertia.ixx");
139+
config.names.emplace_back(tf_prefix + "payload/inertia.iyy");
140+
config.names.emplace_back(tf_prefix + "payload/inertia.izz");
141+
config.names.emplace_back(tf_prefix + "payload/inertia.ixy");
142+
config.names.emplace_back(tf_prefix + "payload/inertia.ixz");
143+
config.names.emplace_back(tf_prefix + "payload/inertia.iyz");
144+
config.names.emplace_back(tf_prefix + "payload/transition_time");
145+
138146
return config;
139147
}
140148

@@ -200,6 +208,12 @@ controller_interface::InterfaceConfiguration ur_controllers::GPIOController::sta
200208
config.names.emplace_back(tf_prefix + "payload/cog.x");
201209
config.names.emplace_back(tf_prefix + "payload/cog.y");
202210
config.names.emplace_back(tf_prefix + "payload/cog.z");
211+
config.names.emplace_back(tf_prefix + "payload/inertia.ixx");
212+
config.names.emplace_back(tf_prefix + "payload/inertia.iyy");
213+
config.names.emplace_back(tf_prefix + "payload/inertia.izz");
214+
config.names.emplace_back(tf_prefix + "payload/inertia.ixy");
215+
config.names.emplace_back(tf_prefix + "payload/inertia.ixz");
216+
config.names.emplace_back(tf_prefix + "payload/inertia.iyz");
203217

204218
return config;
205219
}
@@ -618,6 +632,15 @@ bool GPIOController::setPayload(const ur_msgs::srv::SetPayload::Request::SharedP
618632
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_X].set_value(req->center_of_gravity.x);
619633
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_Y].set_value(req->center_of_gravity.y);
620634
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_Z].set_value(req->center_of_gravity.z);
635+
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IXX].set_value(req->ixx);
636+
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IYY].set_value(req->iyy);
637+
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IZZ].set_value(req->izz);
638+
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IXY].set_value(req->ixy);
639+
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IXZ].set_value(req->ixz);
640+
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IYZ].set_value(req->iyz);
641+
double transition_time_seconds =
642+
static_cast<double>(req->transition_time.sec) + (static_cast<double>(req->transition_time.nanosec) * 1e-9);
643+
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_TRANSITION_TIME].set_value(transition_time_seconds);
621644

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

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

709-
bool GPIOController::waitForPayloadRtdeMatch(double mass, double cx, double cy, double cz)
734+
bool GPIOController::waitForPayloadRtdeMatch(double mass, double cx, double cy, double cz, double ixx, double iyy,
735+
double izz, double ixy, double ixz, double iyz, double transition_time)
710736
{
711737
constexpr double tol_mass = 1e-3;
712738
constexpr double tol_cog = 1e-4;
739+
constexpr double tol_inertia = 1e-4;
713740
const auto maximum_retries = params_.check_io_successfull_retries;
714741

742+
if (transition_time > 0.0) {
743+
std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(transition_time * 1000)));
744+
}
745+
715746
for (int retries = 0; retries <= maximum_retries; ++retries) {
716747
const auto m = state_interfaces_[StateInterfaces::PAYLOAD_STATE_MASS].get_optional();
717748
const auto sx = state_interfaces_[StateInterfaces::PAYLOAD_STATE_COG_X].get_optional();
718749
const auto sy = state_interfaces_[StateInterfaces::PAYLOAD_STATE_COG_Y].get_optional();
719750
const auto sz = state_interfaces_[StateInterfaces::PAYLOAD_STATE_COG_Z].get_optional();
720-
if (m && sx && sy && sz) {
751+
const auto sixx = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IXX].get_optional();
752+
const auto siyy = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IYY].get_optional();
753+
const auto sizz = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IZZ].get_optional();
754+
const auto sixy = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IXY].get_optional();
755+
const auto sixz = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IXZ].get_optional();
756+
const auto siyz = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IYZ].get_optional();
757+
if (m && sx && sy && sz && sixx && siyy && sizz && sixy && sixz && siyz) {
721758
if (std::abs(*m - mass) <= tol_mass && std::abs(*sx - cx) <= tol_cog && std::abs(*sy - cy) <= tol_cog &&
722-
std::abs(*sz - cz) <= tol_cog) {
759+
std::abs(*sz - cz) <= tol_cog && std::abs(*sixx - ixx) <= tol_inertia &&
760+
std::abs(*siyy - iyy) <= tol_inertia && std::abs(*sizz - izz) <= tol_inertia &&
761+
std::abs(*sixy - ixy) <= tol_inertia && std::abs(*sixz - ixz) <= tol_inertia &&
762+
std::abs(*siyz - iyz) <= tol_inertia) {
723763
return true;
724764
}
725765
}

‎ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -284,10 +284,13 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
284284

285285
// Payload stuff
286286
urcl::vector3d_t payload_center_of_gravity_;
287+
urcl::vector6d_t payload_inertia_;
287288
double payload_mass_;
289+
double payload_transition_time_;
288290
double payload_async_success_;
289291
double rtde_payload_mass_ = 0.0;
290292
urcl::vector3d_t rtde_payload_cog_{ 0.0, 0.0, 0.0 };
293+
urcl::vector6d_t rtde_payload_inertia_{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
291294

292295
// Friction model parameters
293296
urcl::vector6d_t friction_model_viscous_;

‎ur_robot_driver/resources/rtde_output_recipe.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,3 +29,4 @@ actual_current
2929
tcp_offset
3030
payload
3131
payload_cog
32+
payload_inertia

‎ur_robot_driver/src/hardware_interface.cpp‎

Lines changed: 37 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -415,7 +415,18 @@ std::vector<hardware_interface::StateInterface> URPositionHardwareInterface::exp
415415
hardware_interface::StateInterface(tf_prefix + "payload", "cog.y", &rtde_payload_cog_[1]));
416416
state_interfaces.emplace_back(
417417
hardware_interface::StateInterface(tf_prefix + "payload", "cog.z", &rtde_payload_cog_[2]));
418-
418+
state_interfaces.emplace_back(
419+
hardware_interface::StateInterface(tf_prefix + "payload", "inertia.ixx", &rtde_payload_inertia_[0]));
420+
state_interfaces.emplace_back(
421+
hardware_interface::StateInterface(tf_prefix + "payload", "inertia.iyy", &rtde_payload_inertia_[1]));
422+
state_interfaces.emplace_back(
423+
hardware_interface::StateInterface(tf_prefix + "payload", "inertia.izz", &rtde_payload_inertia_[2]));
424+
state_interfaces.emplace_back(
425+
hardware_interface::StateInterface(tf_prefix + "payload", "inertia.ixy", &rtde_payload_inertia_[3]));
426+
state_interfaces.emplace_back(
427+
hardware_interface::StateInterface(tf_prefix + "payload", "inertia.ixz", &rtde_payload_inertia_[4]));
428+
state_interfaces.emplace_back(
429+
hardware_interface::StateInterface(tf_prefix + "payload", "inertia.iyz", &rtde_payload_inertia_[5]));
419430
// Motion primitives stuff
420431
state_interfaces.emplace_back(hardware_interface::StateInterface(tf_prefix + HW_IF_MOTION_PRIMITIVES,
421432
"execution_status", &hw_moprim_states_[0]));
@@ -478,6 +489,20 @@ std::vector<hardware_interface::CommandInterface> URPositionHardwareInterface::e
478489
hardware_interface::CommandInterface(tf_prefix + "payload", "cog.y", &payload_center_of_gravity_[1]));
479490
command_interfaces.emplace_back(
480491
hardware_interface::CommandInterface(tf_prefix + "payload", "cog.z", &payload_center_of_gravity_[2]));
492+
command_interfaces.emplace_back(
493+
hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.ixx", &payload_inertia_[0]));
494+
command_interfaces.emplace_back(
495+
hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.iyy", &payload_inertia_[1]));
496+
command_interfaces.emplace_back(
497+
hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.izz", &payload_inertia_[2]));
498+
command_interfaces.emplace_back(
499+
hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.ixy", &payload_inertia_[3]));
500+
command_interfaces.emplace_back(
501+
hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.ixz", &payload_inertia_[4]));
502+
command_interfaces.emplace_back(
503+
hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.iyz", &payload_inertia_[5]));
504+
command_interfaces.emplace_back(
505+
hardware_interface::CommandInterface(tf_prefix + "payload", "transition_time", &payload_transition_time_));
481506
command_interfaces.emplace_back(
482507
hardware_interface::CommandInterface(tf_prefix + "payload", "payload_async_success", &payload_async_success_));
483508

@@ -999,6 +1024,7 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp::
9991024
readData(data_package_buffer_, "tcp_offset", tcp_offset_);
10001025
readData(data_package_buffer_, "payload", rtde_payload_mass_);
10011026
readData(data_package_buffer_, "payload_cog", rtde_payload_cog_);
1027+
readData(data_package_buffer_, "payload_inertia", rtde_payload_inertia_);
10021028

10031029
// required transforms
10041030
extractToolPose();
@@ -1138,6 +1164,8 @@ void URPositionHardwareInterface::initAsyncIO()
11381164

11391165
payload_mass_ = NO_NEW_CMD_;
11401166
payload_center_of_gravity_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
1167+
payload_inertia_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
1168+
payload_transition_time_ = NO_NEW_CMD_;
11411169

11421170
gravity_vector_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
11431171
friction_model_viscous_.fill(NO_NEW_CMD_);
@@ -1205,10 +1233,16 @@ void URPositionHardwareInterface::checkAsyncIO()
12051233

12061234
if (!std::isnan(payload_mass_) && !std::isnan(payload_center_of_gravity_[0]) &&
12071235
!std::isnan(payload_center_of_gravity_[1]) && !std::isnan(payload_center_of_gravity_[2]) &&
1208-
ur_driver_ != nullptr) {
1209-
payload_async_success_ = ur_driver_->setPayload(payload_mass_, payload_center_of_gravity_);
1236+
!std::isnan(payload_inertia_[0]) && !std::isnan(payload_inertia_[1]) && !std::isnan(payload_inertia_[2]) &&
1237+
!std::isnan(payload_inertia_[3]) && !std::isnan(payload_inertia_[4]) && !std::isnan(payload_inertia_[5]) &&
1238+
!std::isnan(payload_transition_time_) && ur_driver_ != nullptr) {
1239+
payload_async_success_ = ur_driver_->setTargetPayload(payload_mass_, payload_center_of_gravity_, payload_inertia_,
1240+
payload_transition_time_);
1241+
12101242
payload_mass_ = NO_NEW_CMD_;
12111243
payload_center_of_gravity_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
1244+
payload_inertia_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
1245+
payload_transition_time_ = NO_NEW_CMD_;
12121246
}
12131247

12141248
if (!std::isnan(gravity_vector_[0]) && !std::isnan(gravity_vector_[1]) && !std::isnan(gravity_vector_[2]) &&

‎ur_robot_driver/test/integration_test_io_controller.py‎

Lines changed: 62 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -33,13 +33,14 @@
3333
import time
3434
import unittest
3535

36-
import launch_testing
3736
import pytest
3837
import rclpy
3938
from geometry_msgs.msg import Vector3
4039
from rclpy.node import Node
4140
from ur_msgs.msg import IOStates
4241

42+
from builtin_interfaces.msg import Duration as DurationMsg
43+
4344
sys.path.append(os.path.dirname(__file__))
4445
from test_common import ( # noqa: E402
4546
ControllerManagerInterface,
@@ -50,9 +51,8 @@
5051

5152

5253
@pytest.mark.launch_test
53-
@launch_testing.parametrize("tf_prefix", [(""), ("my_ur_")])
54-
def generate_test_description(tf_prefix):
55-
return generate_driver_test_description(tf_prefix=tf_prefix)
54+
def generate_test_description():
55+
return generate_driver_test_description()
5656

5757

5858
class IOControllerTest(unittest.TestCase):
@@ -159,3 +159,61 @@ def test_set_payload(self):
159159
mass=0.0, center_of_gravity=Vector3(x=0.0, y=0.0, z=0.0)
160160
)
161161
self.assertTrue(result.success, "Resetting payload via set_payload failed")
162+
163+
def test_set_payload_with_inertia(self):
164+
"""Setting mass, COG and full inertia matrix should succeed."""
165+
res = self._io_status_controller_interface.set_payload(
166+
mass=1.0,
167+
center_of_gravity=Vector3(x=0.1, y=0.0, z=0.2),
168+
ixx=0.01,
169+
iyy=0.01,
170+
izz=0.02,
171+
ixy=0.0,
172+
ixz=0.0,
173+
iyz=0.0,
174+
transition_time=DurationMsg(),
175+
)
176+
self.assertTrue(res.success)
177+
178+
def test_set_payload_with_transition_time(self):
179+
"""
180+
Setting payload with transition_time > 0 should succeed.
181+
182+
The service should wait for the transition to complete before verifying.
183+
"""
184+
res = self._io_status_controller_interface.set_payload(
185+
mass=1.0,
186+
center_of_gravity=Vector3(x=0.0, y=0.0, z=0.1),
187+
ixx=0.01,
188+
iyy=0.01,
189+
izz=0.02,
190+
transition_time=DurationMsg(sec=1, nanosec=0),
191+
)
192+
self.assertTrue(res.success)
193+
194+
def test_set_payload_updates_sequentially(self):
195+
"""Multiple sequential set_payload calls should all succeed."""
196+
payloads = [
197+
{"mass": 0.5, "cx": 0.0, "cy": 0.0, "cz": 0.1, "ixx": 0.005, "iyy": 0.005, "izz": 0.01},
198+
{"mass": 1.0, "cx": 0.1, "cy": 0.0, "cz": 0.2, "ixx": 0.01, "iyy": 0.01, "izz": 0.02},
199+
{
200+
"mass": 2.0,
201+
"cx": 0.05,
202+
"cy": 0.05,
203+
"cz": 0.15,
204+
"ixx": 0.02,
205+
"iyy": 0.02,
206+
"izz": 0.04,
207+
},
208+
{"mass": 0.0, "cx": 0.0, "cy": 0.0, "cz": 0.0, "ixx": 0.0, "iyy": 0.0, "izz": 0.0},
209+
]
210+
for p in payloads:
211+
res = self._io_status_controller_interface.set_payload(
212+
mass=p["mass"],
213+
center_of_gravity=Vector3(x=p["cx"], y=p["cy"], z=p["cz"]),
214+
ixx=p["ixx"],
215+
iyy=p["iyy"],
216+
izz=p["izz"],
217+
transition_time=DurationMsg(),
218+
)
219+
self.assertTrue(res.success, f"set_payload failed for m={p['mass']}")

‎ur_robot_driver/urdf/ur.ros2_control.xacro‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -213,12 +213,25 @@
213213
<command_interface name="cog.x"/>
214214
<command_interface name="cog.y"/>
215215
<command_interface name="cog.z"/>
216+
<command_interface name="inertia.ixx"/>
217+
<command_interface name="inertia.iyy"/>
218+
<command_interface name="inertia.izz"/>
219+
<command_interface name="inertia.ixy"/>
220+
<command_interface name="inertia.ixz"/>
221+
<command_interface name="inertia.iyz"/>
222+
<command_interface name="transition_time"/>
216223
<command_interface name="payload_async_success"/>
217224

218225
<state_interface name="mass"/>
219226
<state_interface name="cog.x"/>
220227
<state_interface name="cog.y"/>
221228
<state_interface name="cog.z"/>
229+
<state_interface name="inertia.ixx"/>
230+
<state_interface name="inertia.iyy"/>
231+
<state_interface name="inertia.izz"/>
232+
<state_interface name="inertia.ixy"/>
233+
<state_interface name="inertia.ixz"/>
234+
<state_interface name="inertia.iyz"/>
222235
</gpio>
223236

224237
<gpio name="${tf_prefix}gravity">

0 commit comments

Comments
 (0)